Joe Verbout / Mbed 2 deprecated main

opencv on mbed

Dependencies:   mbed

opencv2/core/affine.hpp@0:ea44dc9ed014, 2016-03-31 (annotated)

Committer:
joeverbout
Date:
Thu Mar 31 21:16:38 2016 +0000
Revision:
0:ea44dc9ed014
OpenCV on mbed attempt

Who changed what in which revision?

UserRevisionLine numberNew contents of line
joeverbout 0:ea44dc9ed014 1 /*M///////////////////////////////////////////////////////////////////////////////////////
joeverbout 0:ea44dc9ed014 2 //
joeverbout 0:ea44dc9ed014 3 // IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
joeverbout 0:ea44dc9ed014 4 //
joeverbout 0:ea44dc9ed014 5 // By downloading, copying, installing or using the software you agree to this license.
joeverbout 0:ea44dc9ed014 6 // If you do not agree to this license, do not download, install,
joeverbout 0:ea44dc9ed014 7 // copy or use the software.
joeverbout 0:ea44dc9ed014 8 //
joeverbout 0:ea44dc9ed014 9 //
joeverbout 0:ea44dc9ed014 10 // License Agreement
joeverbout 0:ea44dc9ed014 11 // For Open Source Computer Vision Library
joeverbout 0:ea44dc9ed014 12 //
joeverbout 0:ea44dc9ed014 13 // Copyright (C) 2000-2008, Intel Corporation, all rights reserved.
joeverbout 0:ea44dc9ed014 14 // Copyright (C) 2009, Willow Garage Inc., all rights reserved.
joeverbout 0:ea44dc9ed014 15 // Copyright (C) 2013, OpenCV Foundation, all rights reserved.
joeverbout 0:ea44dc9ed014 16 // Third party copyrights are property of their respective owners.
joeverbout 0:ea44dc9ed014 17 //
joeverbout 0:ea44dc9ed014 18 // Redistribution and use in source and binary forms, with or without modification,
joeverbout 0:ea44dc9ed014 19 // are permitted provided that the following conditions are met:
joeverbout 0:ea44dc9ed014 20 //
joeverbout 0:ea44dc9ed014 21 // * Redistribution's of source code must retain the above copyright notice,
joeverbout 0:ea44dc9ed014 22 // this list of conditions and the following disclaimer.
joeverbout 0:ea44dc9ed014 23 //
joeverbout 0:ea44dc9ed014 24 // * Redistribution's in binary form must reproduce the above copyright notice,
joeverbout 0:ea44dc9ed014 25 // this list of conditions and the following disclaimer in the documentation
joeverbout 0:ea44dc9ed014 26 // and/or other materials provided with the distribution.
joeverbout 0:ea44dc9ed014 27 //
joeverbout 0:ea44dc9ed014 28 // * The name of the copyright holders may not be used to endorse or promote products
joeverbout 0:ea44dc9ed014 29 // derived from this software without specific prior written permission.
joeverbout 0:ea44dc9ed014 30 //
joeverbout 0:ea44dc9ed014 31 // This software is provided by the copyright holders and contributors "as is" and
joeverbout 0:ea44dc9ed014 32 // any express or implied warranties, including, but not limited to, the implied
joeverbout 0:ea44dc9ed014 33 // warranties of merchantability and fitness for a particular purpose are disclaimed.
joeverbout 0:ea44dc9ed014 34 // In no event shall the Intel Corporation or contributors be liable for any direct,
joeverbout 0:ea44dc9ed014 35 // indirect, incidental, special, exemplary, or consequential damages
joeverbout 0:ea44dc9ed014 36 // (including, but not limited to, procurement of substitute goods or services;
joeverbout 0:ea44dc9ed014 37 // loss of use, data, or profits; or business interruption) however caused
joeverbout 0:ea44dc9ed014 38 // and on any theory of liability, whether in contract, strict liability,
joeverbout 0:ea44dc9ed014 39 // or tort (including negligence or otherwise) arising in any way out of
joeverbout 0:ea44dc9ed014 40 // the use of this software, even if advised of the possibility of such damage.
joeverbout 0:ea44dc9ed014 41 //
joeverbout 0:ea44dc9ed014 42 //M*/
joeverbout 0:ea44dc9ed014 43
joeverbout 0:ea44dc9ed014 44 #ifndef __OPENCV_CORE_AFFINE3_HPP__
joeverbout 0:ea44dc9ed014 45 #define __OPENCV_CORE_AFFINE3_HPP__
joeverbout 0:ea44dc9ed014 46
joeverbout 0:ea44dc9ed014 47 #ifdef __cplusplus
joeverbout 0:ea44dc9ed014 48
joeverbout 0:ea44dc9ed014 49 #include <opencv2/core.hpp>
joeverbout 0:ea44dc9ed014 50
joeverbout 0:ea44dc9ed014 51 namespace cv
joeverbout 0:ea44dc9ed014 52 {
joeverbout 0:ea44dc9ed014 53
joeverbout 0:ea44dc9ed014 54 //! @addtogroup core
joeverbout 0:ea44dc9ed014 55 //! @{
joeverbout 0:ea44dc9ed014 56
joeverbout 0:ea44dc9ed014 57 /** @brief Affine transform
joeverbout 0:ea44dc9ed014 58 @todo document
joeverbout 0:ea44dc9ed014 59 */
joeverbout 0:ea44dc9ed014 60 template<typename T>
joeverbout 0:ea44dc9ed014 61 class Affine3
joeverbout 0:ea44dc9ed014 62 {
joeverbout 0:ea44dc9ed014 63 public:
joeverbout 0:ea44dc9ed014 64 typedef T float_type;
joeverbout 0:ea44dc9ed014 65 typedef Matx<float_type, 3, 3> Mat3;
joeverbout 0:ea44dc9ed014 66 typedef Matx<float_type, 4, 4> Mat4;
joeverbout 0:ea44dc9ed014 67 typedef Vec<float_type, 3> Vec3;
joeverbout 0:ea44dc9ed014 68
joeverbout 0:ea44dc9ed014 69 Affine3();
joeverbout 0:ea44dc9ed014 70
joeverbout 0:ea44dc9ed014 71 //! Augmented affine matrix
joeverbout 0:ea44dc9ed014 72 Affine3(const Mat4& affine);
joeverbout 0:ea44dc9ed014 73
joeverbout 0:ea44dc9ed014 74 //! Rotation matrix
joeverbout 0:ea44dc9ed014 75 Affine3(const Mat3& R, const Vec3& t = Vec3::all(0));
joeverbout 0:ea44dc9ed014 76
joeverbout 0:ea44dc9ed014 77 //! Rodrigues vector
joeverbout 0:ea44dc9ed014 78 Affine3(const Vec3& rvec, const Vec3& t = Vec3::all(0));
joeverbout 0:ea44dc9ed014 79
joeverbout 0:ea44dc9ed014 80 //! Combines all contructors above. Supports 4x4, 4x3, 3x3, 1x3, 3x1 sizes of data matrix
joeverbout 0:ea44dc9ed014 81 explicit Affine3(const Mat& data, const Vec3& t = Vec3::all(0));
joeverbout 0:ea44dc9ed014 82
joeverbout 0:ea44dc9ed014 83 //! From 16th element array
joeverbout 0:ea44dc9ed014 84 explicit Affine3(const float_type* vals);
joeverbout 0:ea44dc9ed014 85
joeverbout 0:ea44dc9ed014 86 //! Create identity transform
joeverbout 0:ea44dc9ed014 87 static Affine3 Identity();
joeverbout 0:ea44dc9ed014 88
joeverbout 0:ea44dc9ed014 89 //! Rotation matrix
joeverbout 0:ea44dc9ed014 90 void rotation(const Mat3& R);
joeverbout 0:ea44dc9ed014 91
joeverbout 0:ea44dc9ed014 92 //! Rodrigues vector
joeverbout 0:ea44dc9ed014 93 void rotation(const Vec3& rvec);
joeverbout 0:ea44dc9ed014 94
joeverbout 0:ea44dc9ed014 95 //! Combines rotation methods above. Suports 3x3, 1x3, 3x1 sizes of data matrix;
joeverbout 0:ea44dc9ed014 96 void rotation(const Mat& data);
joeverbout 0:ea44dc9ed014 97
joeverbout 0:ea44dc9ed014 98 void linear(const Mat3& L);
joeverbout 0:ea44dc9ed014 99 void translation(const Vec3& t);
joeverbout 0:ea44dc9ed014 100
joeverbout 0:ea44dc9ed014 101 Mat3 rotation() const;
joeverbout 0:ea44dc9ed014 102 Mat3 linear() const;
joeverbout 0:ea44dc9ed014 103 Vec3 translation() const;
joeverbout 0:ea44dc9ed014 104
joeverbout 0:ea44dc9ed014 105 //! Rodrigues vector
joeverbout 0:ea44dc9ed014 106 Vec3 rvec() const;
joeverbout 0:ea44dc9ed014 107
joeverbout 0:ea44dc9ed014 108 Affine3 inv(int method = cv::DECOMP_SVD) const;
joeverbout 0:ea44dc9ed014 109
joeverbout 0:ea44dc9ed014 110 //! a.rotate(R) is equivalent to Affine(R, 0) * a;
joeverbout 0:ea44dc9ed014 111 Affine3 rotate(const Mat3& R) const;
joeverbout 0:ea44dc9ed014 112
joeverbout 0:ea44dc9ed014 113 //! a.rotate(rvec) is equivalent to Affine(rvec, 0) * a;
joeverbout 0:ea44dc9ed014 114 Affine3 rotate(const Vec3& rvec) const;
joeverbout 0:ea44dc9ed014 115
joeverbout 0:ea44dc9ed014 116 //! a.translate(t) is equivalent to Affine(E, t) * a;
joeverbout 0:ea44dc9ed014 117 Affine3 translate(const Vec3& t) const;
joeverbout 0:ea44dc9ed014 118
joeverbout 0:ea44dc9ed014 119 //! a.concatenate(affine) is equivalent to affine * a;
joeverbout 0:ea44dc9ed014 120 Affine3 concatenate(const Affine3& affine) const;
joeverbout 0:ea44dc9ed014 121
joeverbout 0:ea44dc9ed014 122 template <typename Y> operator Affine3<Y>() const;
joeverbout 0:ea44dc9ed014 123
joeverbout 0:ea44dc9ed014 124 template <typename Y> Affine3<Y> cast() const;
joeverbout 0:ea44dc9ed014 125
joeverbout 0:ea44dc9ed014 126 Mat4 matrix;
joeverbout 0:ea44dc9ed014 127
joeverbout 0:ea44dc9ed014 128 #if defined EIGEN_WORLD_VERSION && defined EIGEN_GEOMETRY_MODULE_H
joeverbout 0:ea44dc9ed014 129 Affine3(const Eigen::Transform<T, 3, Eigen::Affine, (Eigen::RowMajor)>& affine);
joeverbout 0:ea44dc9ed014 130 Affine3(const Eigen::Transform<T, 3, Eigen::Affine>& affine);
joeverbout 0:ea44dc9ed014 131 operator Eigen::Transform<T, 3, Eigen::Affine, (Eigen::RowMajor)>() const;
joeverbout 0:ea44dc9ed014 132 operator Eigen::Transform<T, 3, Eigen::Affine>() const;
joeverbout 0:ea44dc9ed014 133 #endif
joeverbout 0:ea44dc9ed014 134 };
joeverbout 0:ea44dc9ed014 135
joeverbout 0:ea44dc9ed014 136 template<typename T> static
joeverbout 0:ea44dc9ed014 137 Affine3<T> operator*(const Affine3<T>& affine1, const Affine3<T>& affine2);
joeverbout 0:ea44dc9ed014 138
joeverbout 0:ea44dc9ed014 139 template<typename T, typename V> static
joeverbout 0:ea44dc9ed014 140 V operator*(const Affine3<T>& affine, const V& vector);
joeverbout 0:ea44dc9ed014 141
joeverbout 0:ea44dc9ed014 142 typedef Affine3<float> Affine3f;
joeverbout 0:ea44dc9ed014 143 typedef Affine3<double> Affine3d;
joeverbout 0:ea44dc9ed014 144
joeverbout 0:ea44dc9ed014 145 static Vec3f operator*(const Affine3f& affine, const Vec3f& vector);
joeverbout 0:ea44dc9ed014 146 static Vec3d operator*(const Affine3d& affine, const Vec3d& vector);
joeverbout 0:ea44dc9ed014 147
joeverbout 0:ea44dc9ed014 148 template<typename _Tp> class DataType< Affine3<_Tp> >
joeverbout 0:ea44dc9ed014 149 {
joeverbout 0:ea44dc9ed014 150 public:
joeverbout 0:ea44dc9ed014 151 typedef Affine3<_Tp> value_type;
joeverbout 0:ea44dc9ed014 152 typedef Affine3<typename DataType<_Tp>::work_type> work_type;
joeverbout 0:ea44dc9ed014 153 typedef _Tp channel_type;
joeverbout 0:ea44dc9ed014 154
joeverbout 0:ea44dc9ed014 155 enum { generic_type = 0,
joeverbout 0:ea44dc9ed014 156 depth = DataType<channel_type>::depth,
joeverbout 0:ea44dc9ed014 157 channels = 16,
joeverbout 0:ea44dc9ed014 158 fmt = DataType<channel_type>::fmt + ((channels - 1) << 8),
joeverbout 0:ea44dc9ed014 159 type = CV_MAKETYPE(depth, channels)
joeverbout 0:ea44dc9ed014 160 };
joeverbout 0:ea44dc9ed014 161
joeverbout 0:ea44dc9ed014 162 typedef Vec<channel_type, channels> vec_type;
joeverbout 0:ea44dc9ed014 163 };
joeverbout 0:ea44dc9ed014 164
joeverbout 0:ea44dc9ed014 165 //! @} core
joeverbout 0:ea44dc9ed014 166
joeverbout 0:ea44dc9ed014 167 }
joeverbout 0:ea44dc9ed014 168
joeverbout 0:ea44dc9ed014 169 //! @cond IGNORED
joeverbout 0:ea44dc9ed014 170
joeverbout 0:ea44dc9ed014 171 ///////////////////////////////////////////////////////////////////////////////////
joeverbout 0:ea44dc9ed014 172 // Implementaiton
joeverbout 0:ea44dc9ed014 173
joeverbout 0:ea44dc9ed014 174 template<typename T> inline
joeverbout 0:ea44dc9ed014 175 cv::Affine3<T>::Affine3()
joeverbout 0:ea44dc9ed014 176 : matrix(Mat4::eye())
joeverbout 0:ea44dc9ed014 177 {}
joeverbout 0:ea44dc9ed014 178
joeverbout 0:ea44dc9ed014 179 template<typename T> inline
joeverbout 0:ea44dc9ed014 180 cv::Affine3<T>::Affine3(const Mat4& affine)
joeverbout 0:ea44dc9ed014 181 : matrix(affine)
joeverbout 0:ea44dc9ed014 182 {}
joeverbout 0:ea44dc9ed014 183
joeverbout 0:ea44dc9ed014 184 template<typename T> inline
joeverbout 0:ea44dc9ed014 185 cv::Affine3<T>::Affine3(const Mat3& R, const Vec3& t)
joeverbout 0:ea44dc9ed014 186 {
joeverbout 0:ea44dc9ed014 187 rotation(R);
joeverbout 0:ea44dc9ed014 188 translation(t);
joeverbout 0:ea44dc9ed014 189 matrix.val[12] = matrix.val[13] = matrix.val[14] = 0;
joeverbout 0:ea44dc9ed014 190 matrix.val[15] = 1;
joeverbout 0:ea44dc9ed014 191 }
joeverbout 0:ea44dc9ed014 192
joeverbout 0:ea44dc9ed014 193 template<typename T> inline
joeverbout 0:ea44dc9ed014 194 cv::Affine3<T>::Affine3(const Vec3& _rvec, const Vec3& t)
joeverbout 0:ea44dc9ed014 195 {
joeverbout 0:ea44dc9ed014 196 rotation(_rvec);
joeverbout 0:ea44dc9ed014 197 translation(t);
joeverbout 0:ea44dc9ed014 198 matrix.val[12] = matrix.val[13] = matrix.val[14] = 0;
joeverbout 0:ea44dc9ed014 199 matrix.val[15] = 1;
joeverbout 0:ea44dc9ed014 200 }
joeverbout 0:ea44dc9ed014 201
joeverbout 0:ea44dc9ed014 202 template<typename T> inline
joeverbout 0:ea44dc9ed014 203 cv::Affine3<T>::Affine3(const cv::Mat& data, const Vec3& t)
joeverbout 0:ea44dc9ed014 204 {
joeverbout 0:ea44dc9ed014 205 CV_Assert(data.type() == cv::DataType<T>::type);
joeverbout 0:ea44dc9ed014 206
joeverbout 0:ea44dc9ed014 207 if (data.cols == 4 && data.rows == 4)
joeverbout 0:ea44dc9ed014 208 {
joeverbout 0:ea44dc9ed014 209 data.copyTo(matrix);
joeverbout 0:ea44dc9ed014 210 return;
joeverbout 0:ea44dc9ed014 211 }
joeverbout 0:ea44dc9ed014 212 else if (data.cols == 4 && data.rows == 3)
joeverbout 0:ea44dc9ed014 213 {
joeverbout 0:ea44dc9ed014 214 rotation(data(Rect(0, 0, 3, 3)));
joeverbout 0:ea44dc9ed014 215 translation(data(Rect(3, 0, 1, 3)));
joeverbout 0:ea44dc9ed014 216 return;
joeverbout 0:ea44dc9ed014 217 }
joeverbout 0:ea44dc9ed014 218
joeverbout 0:ea44dc9ed014 219 rotation(data);
joeverbout 0:ea44dc9ed014 220 translation(t);
joeverbout 0:ea44dc9ed014 221 matrix.val[12] = matrix.val[13] = matrix.val[14] = 0;
joeverbout 0:ea44dc9ed014 222 matrix.val[15] = 1;
joeverbout 0:ea44dc9ed014 223 }
joeverbout 0:ea44dc9ed014 224
joeverbout 0:ea44dc9ed014 225 template<typename T> inline
joeverbout 0:ea44dc9ed014 226 cv::Affine3<T>::Affine3(const float_type* vals) : matrix(vals)
joeverbout 0:ea44dc9ed014 227 {}
joeverbout 0:ea44dc9ed014 228
joeverbout 0:ea44dc9ed014 229 template<typename T> inline
joeverbout 0:ea44dc9ed014 230 cv::Affine3<T> cv::Affine3<T>::Identity()
joeverbout 0:ea44dc9ed014 231 {
joeverbout 0:ea44dc9ed014 232 return Affine3<T>(cv::Affine3<T>::Mat4::eye());
joeverbout 0:ea44dc9ed014 233 }
joeverbout 0:ea44dc9ed014 234
joeverbout 0:ea44dc9ed014 235 template<typename T> inline
joeverbout 0:ea44dc9ed014 236 void cv::Affine3<T>::rotation(const Mat3& R)
joeverbout 0:ea44dc9ed014 237 {
joeverbout 0:ea44dc9ed014 238 linear(R);
joeverbout 0:ea44dc9ed014 239 }
joeverbout 0:ea44dc9ed014 240
joeverbout 0:ea44dc9ed014 241 template<typename T> inline
joeverbout 0:ea44dc9ed014 242 void cv::Affine3<T>::rotation(const Vec3& _rvec)
joeverbout 0:ea44dc9ed014 243 {
joeverbout 0:ea44dc9ed014 244 double rx = _rvec[0], ry = _rvec[1], rz = _rvec[2];
joeverbout 0:ea44dc9ed014 245 double theta = std::sqrt(rx*rx + ry*ry + rz*rz);
joeverbout 0:ea44dc9ed014 246
joeverbout 0:ea44dc9ed014 247 if (theta < DBL_EPSILON)
joeverbout 0:ea44dc9ed014 248 rotation(Mat3::eye());
joeverbout 0:ea44dc9ed014 249 else
joeverbout 0:ea44dc9ed014 250 {
joeverbout 0:ea44dc9ed014 251 const double I[] = { 1, 0, 0, 0, 1, 0, 0, 0, 1 };
joeverbout 0:ea44dc9ed014 252
joeverbout 0:ea44dc9ed014 253 double c = std::cos(theta);
joeverbout 0:ea44dc9ed014 254 double s = std::sin(theta);
joeverbout 0:ea44dc9ed014 255 double c1 = 1. - c;
joeverbout 0:ea44dc9ed014 256 double itheta = (theta != 0) ? 1./theta : 0.;
joeverbout 0:ea44dc9ed014 257
joeverbout 0:ea44dc9ed014 258 rx *= itheta; ry *= itheta; rz *= itheta;
joeverbout 0:ea44dc9ed014 259
joeverbout 0:ea44dc9ed014 260 double rrt[] = { rx*rx, rx*ry, rx*rz, rx*ry, ry*ry, ry*rz, rx*rz, ry*rz, rz*rz };
joeverbout 0:ea44dc9ed014 261 double _r_x_[] = { 0, -rz, ry, rz, 0, -rx, -ry, rx, 0 };
joeverbout 0:ea44dc9ed014 262 Mat3 R;
joeverbout 0:ea44dc9ed014 263
joeverbout 0:ea44dc9ed014 264 // R = cos(theta)*I + (1 - cos(theta))*r*rT + sin(theta)*[r_x]
joeverbout 0:ea44dc9ed014 265 // where [r_x] is [0 -rz ry; rz 0 -rx; -ry rx 0]
joeverbout 0:ea44dc9ed014 266 for(int k = 0; k < 9; ++k)
joeverbout 0:ea44dc9ed014 267 R.val[k] = static_cast<float_type>(c*I[k] + c1*rrt[k] + s*_r_x_[k]);
joeverbout 0:ea44dc9ed014 268
joeverbout 0:ea44dc9ed014 269 rotation(R);
joeverbout 0:ea44dc9ed014 270 }
joeverbout 0:ea44dc9ed014 271 }
joeverbout 0:ea44dc9ed014 272
joeverbout 0:ea44dc9ed014 273 //Combines rotation methods above. Suports 3x3, 1x3, 3x1 sizes of data matrix;
joeverbout 0:ea44dc9ed014 274 template<typename T> inline
joeverbout 0:ea44dc9ed014 275 void cv::Affine3<T>::rotation(const cv::Mat& data)
joeverbout 0:ea44dc9ed014 276 {
joeverbout 0:ea44dc9ed014 277 CV_Assert(data.type() == cv::DataType<T>::type);
joeverbout 0:ea44dc9ed014 278
joeverbout 0:ea44dc9ed014 279 if (data.cols == 3 && data.rows == 3)
joeverbout 0:ea44dc9ed014 280 {
joeverbout 0:ea44dc9ed014 281 Mat3 R;
joeverbout 0:ea44dc9ed014 282 data.copyTo(R);
joeverbout 0:ea44dc9ed014 283 rotation(R);
joeverbout 0:ea44dc9ed014 284 }
joeverbout 0:ea44dc9ed014 285 else if ((data.cols == 3 && data.rows == 1) || (data.cols == 1 && data.rows == 3))
joeverbout 0:ea44dc9ed014 286 {
joeverbout 0:ea44dc9ed014 287 Vec3 _rvec;
joeverbout 0:ea44dc9ed014 288 data.reshape(1, 3).copyTo(_rvec);
joeverbout 0:ea44dc9ed014 289 rotation(_rvec);
joeverbout 0:ea44dc9ed014 290 }
joeverbout 0:ea44dc9ed014 291 else
joeverbout 0:ea44dc9ed014 292 CV_Assert(!"Input marix can be 3x3, 1x3 or 3x1");
joeverbout 0:ea44dc9ed014 293 }
joeverbout 0:ea44dc9ed014 294
joeverbout 0:ea44dc9ed014 295 template<typename T> inline
joeverbout 0:ea44dc9ed014 296 void cv::Affine3<T>::linear(const Mat3& L)
joeverbout 0:ea44dc9ed014 297 {
joeverbout 0:ea44dc9ed014 298 matrix.val[0] = L.val[0]; matrix.val[1] = L.val[1]; matrix.val[ 2] = L.val[2];
joeverbout 0:ea44dc9ed014 299 matrix.val[4] = L.val[3]; matrix.val[5] = L.val[4]; matrix.val[ 6] = L.val[5];
joeverbout 0:ea44dc9ed014 300 matrix.val[8] = L.val[6]; matrix.val[9] = L.val[7]; matrix.val[10] = L.val[8];
joeverbout 0:ea44dc9ed014 301 }
joeverbout 0:ea44dc9ed014 302
joeverbout 0:ea44dc9ed014 303 template<typename T> inline
joeverbout 0:ea44dc9ed014 304 void cv::Affine3<T>::translation(const Vec3& t)
joeverbout 0:ea44dc9ed014 305 {
joeverbout 0:ea44dc9ed014 306 matrix.val[3] = t[0]; matrix.val[7] = t[1]; matrix.val[11] = t[2];
joeverbout 0:ea44dc9ed014 307 }
joeverbout 0:ea44dc9ed014 308
joeverbout 0:ea44dc9ed014 309 template<typename T> inline
joeverbout 0:ea44dc9ed014 310 typename cv::Affine3<T>::Mat3 cv::Affine3<T>::rotation() const
joeverbout 0:ea44dc9ed014 311 {
joeverbout 0:ea44dc9ed014 312 return linear();
joeverbout 0:ea44dc9ed014 313 }
joeverbout 0:ea44dc9ed014 314
joeverbout 0:ea44dc9ed014 315 template<typename T> inline
joeverbout 0:ea44dc9ed014 316 typename cv::Affine3<T>::Mat3 cv::Affine3<T>::linear() const
joeverbout 0:ea44dc9ed014 317 {
joeverbout 0:ea44dc9ed014 318 typename cv::Affine3<T>::Mat3 R;
joeverbout 0:ea44dc9ed014 319 R.val[0] = matrix.val[0]; R.val[1] = matrix.val[1]; R.val[2] = matrix.val[ 2];
joeverbout 0:ea44dc9ed014 320 R.val[3] = matrix.val[4]; R.val[4] = matrix.val[5]; R.val[5] = matrix.val[ 6];
joeverbout 0:ea44dc9ed014 321 R.val[6] = matrix.val[8]; R.val[7] = matrix.val[9]; R.val[8] = matrix.val[10];
joeverbout 0:ea44dc9ed014 322 return R;
joeverbout 0:ea44dc9ed014 323 }
joeverbout 0:ea44dc9ed014 324
joeverbout 0:ea44dc9ed014 325 template<typename T> inline
joeverbout 0:ea44dc9ed014 326 typename cv::Affine3<T>::Vec3 cv::Affine3<T>::translation() const
joeverbout 0:ea44dc9ed014 327 {
joeverbout 0:ea44dc9ed014 328 return Vec3(matrix.val[3], matrix.val[7], matrix.val[11]);
joeverbout 0:ea44dc9ed014 329 }
joeverbout 0:ea44dc9ed014 330
joeverbout 0:ea44dc9ed014 331 template<typename T> inline
joeverbout 0:ea44dc9ed014 332 typename cv::Affine3<T>::Vec3 cv::Affine3<T>::rvec() const
joeverbout 0:ea44dc9ed014 333 {
joeverbout 0:ea44dc9ed014 334 cv::Vec3d w;
joeverbout 0:ea44dc9ed014 335 cv::Matx33d u, vt, R = rotation();
joeverbout 0:ea44dc9ed014 336 cv::SVD::compute(R, w, u, vt, cv::SVD::FULL_UV + cv::SVD::MODIFY_A);
joeverbout 0:ea44dc9ed014 337 R = u * vt;
joeverbout 0:ea44dc9ed014 338
joeverbout 0:ea44dc9ed014 339 double rx = R.val[7] - R.val[5];
joeverbout 0:ea44dc9ed014 340 double ry = R.val[2] - R.val[6];
joeverbout 0:ea44dc9ed014 341 double rz = R.val[3] - R.val[1];
joeverbout 0:ea44dc9ed014 342
joeverbout 0:ea44dc9ed014 343 double s = std::sqrt((rx*rx + ry*ry + rz*rz)*0.25);
joeverbout 0:ea44dc9ed014 344 double c = (R.val[0] + R.val[4] + R.val[8] - 1) * 0.5;
joeverbout 0:ea44dc9ed014 345 c = c > 1.0 ? 1.0 : c < -1.0 ? -1.0 : c;
joeverbout 0:ea44dc9ed014 346 double theta = acos(c);
joeverbout 0:ea44dc9ed014 347
joeverbout 0:ea44dc9ed014 348 if( s < 1e-5 )
joeverbout 0:ea44dc9ed014 349 {
joeverbout 0:ea44dc9ed014 350 if( c > 0 )
joeverbout 0:ea44dc9ed014 351 rx = ry = rz = 0;
joeverbout 0:ea44dc9ed014 352 else
joeverbout 0:ea44dc9ed014 353 {
joeverbout 0:ea44dc9ed014 354 double t;
joeverbout 0:ea44dc9ed014 355 t = (R.val[0] + 1) * 0.5;
joeverbout 0:ea44dc9ed014 356 rx = std::sqrt(std::max(t, 0.0));
joeverbout 0:ea44dc9ed014 357 t = (R.val[4] + 1) * 0.5;
joeverbout 0:ea44dc9ed014 358 ry = std::sqrt(std::max(t, 0.0)) * (R.val[1] < 0 ? -1.0 : 1.0);
joeverbout 0:ea44dc9ed014 359 t = (R.val[8] + 1) * 0.5;
joeverbout 0:ea44dc9ed014 360 rz = std::sqrt(std::max(t, 0.0)) * (R.val[2] < 0 ? -1.0 : 1.0);
joeverbout 0:ea44dc9ed014 361
joeverbout 0:ea44dc9ed014 362 if( fabs(rx) < fabs(ry) && fabs(rx) < fabs(rz) && (R.val[5] > 0) != (ry*rz > 0) )
joeverbout 0:ea44dc9ed014 363 rz = -rz;
joeverbout 0:ea44dc9ed014 364 theta /= std::sqrt(rx*rx + ry*ry + rz*rz);
joeverbout 0:ea44dc9ed014 365 rx *= theta;
joeverbout 0:ea44dc9ed014 366 ry *= theta;
joeverbout 0:ea44dc9ed014 367 rz *= theta;
joeverbout 0:ea44dc9ed014 368 }
joeverbout 0:ea44dc9ed014 369 }
joeverbout 0:ea44dc9ed014 370 else
joeverbout 0:ea44dc9ed014 371 {
joeverbout 0:ea44dc9ed014 372 double vth = 1/(2*s);
joeverbout 0:ea44dc9ed014 373 vth *= theta;
joeverbout 0:ea44dc9ed014 374 rx *= vth; ry *= vth; rz *= vth;
joeverbout 0:ea44dc9ed014 375 }
joeverbout 0:ea44dc9ed014 376
joeverbout 0:ea44dc9ed014 377 return cv::Vec3d(rx, ry, rz);
joeverbout 0:ea44dc9ed014 378 }
joeverbout 0:ea44dc9ed014 379
joeverbout 0:ea44dc9ed014 380 template<typename T> inline
joeverbout 0:ea44dc9ed014 381 cv::Affine3<T> cv::Affine3<T>::inv(int method) const
joeverbout 0:ea44dc9ed014 382 {
joeverbout 0:ea44dc9ed014 383 return matrix.inv(method);
joeverbout 0:ea44dc9ed014 384 }
joeverbout 0:ea44dc9ed014 385
joeverbout 0:ea44dc9ed014 386 template<typename T> inline
joeverbout 0:ea44dc9ed014 387 cv::Affine3<T> cv::Affine3<T>::rotate(const Mat3& R) const
joeverbout 0:ea44dc9ed014 388 {
joeverbout 0:ea44dc9ed014 389 Mat3 Lc = linear();
joeverbout 0:ea44dc9ed014 390 Vec3 tc = translation();
joeverbout 0:ea44dc9ed014 391 Mat4 result;
joeverbout 0:ea44dc9ed014 392 result.val[12] = result.val[13] = result.val[14] = 0;
joeverbout 0:ea44dc9ed014 393 result.val[15] = 1;
joeverbout 0:ea44dc9ed014 394
joeverbout 0:ea44dc9ed014 395 for(int j = 0; j < 3; ++j)
joeverbout 0:ea44dc9ed014 396 {
joeverbout 0:ea44dc9ed014 397 for(int i = 0; i < 3; ++i)
joeverbout 0:ea44dc9ed014 398 {
joeverbout 0:ea44dc9ed014 399 float_type value = 0;
joeverbout 0:ea44dc9ed014 400 for(int k = 0; k < 3; ++k)
joeverbout 0:ea44dc9ed014 401 value += R(j, k) * Lc(k, i);
joeverbout 0:ea44dc9ed014 402 result(j, i) = value;
joeverbout 0:ea44dc9ed014 403 }
joeverbout 0:ea44dc9ed014 404
joeverbout 0:ea44dc9ed014 405 result(j, 3) = R.row(j).dot(tc.t());
joeverbout 0:ea44dc9ed014 406 }
joeverbout 0:ea44dc9ed014 407 return result;
joeverbout 0:ea44dc9ed014 408 }
joeverbout 0:ea44dc9ed014 409
joeverbout 0:ea44dc9ed014 410 template<typename T> inline
joeverbout 0:ea44dc9ed014 411 cv::Affine3<T> cv::Affine3<T>::rotate(const Vec3& _rvec) const
joeverbout 0:ea44dc9ed014 412 {
joeverbout 0:ea44dc9ed014 413 return rotate(Affine3f(_rvec).rotation());
joeverbout 0:ea44dc9ed014 414 }
joeverbout 0:ea44dc9ed014 415
joeverbout 0:ea44dc9ed014 416 template<typename T> inline
joeverbout 0:ea44dc9ed014 417 cv::Affine3<T> cv::Affine3<T>::translate(const Vec3& t) const
joeverbout 0:ea44dc9ed014 418 {
joeverbout 0:ea44dc9ed014 419 Mat4 m = matrix;
joeverbout 0:ea44dc9ed014 420 m.val[ 3] += t[0];
joeverbout 0:ea44dc9ed014 421 m.val[ 7] += t[1];
joeverbout 0:ea44dc9ed014 422 m.val[11] += t[2];
joeverbout 0:ea44dc9ed014 423 return m;
joeverbout 0:ea44dc9ed014 424 }
joeverbout 0:ea44dc9ed014 425
joeverbout 0:ea44dc9ed014 426 template<typename T> inline
joeverbout 0:ea44dc9ed014 427 cv::Affine3<T> cv::Affine3<T>::concatenate(const Affine3<T>& affine) const
joeverbout 0:ea44dc9ed014 428 {
joeverbout 0:ea44dc9ed014 429 return (*this).rotate(affine.rotation()).translate(affine.translation());
joeverbout 0:ea44dc9ed014 430 }
joeverbout 0:ea44dc9ed014 431
joeverbout 0:ea44dc9ed014 432 template<typename T> template <typename Y> inline
joeverbout 0:ea44dc9ed014 433 cv::Affine3<T>::operator Affine3<Y>() const
joeverbout 0:ea44dc9ed014 434 {
joeverbout 0:ea44dc9ed014 435 return Affine3<Y>(matrix);
joeverbout 0:ea44dc9ed014 436 }
joeverbout 0:ea44dc9ed014 437
joeverbout 0:ea44dc9ed014 438 template<typename T> template <typename Y> inline
joeverbout 0:ea44dc9ed014 439 cv::Affine3<Y> cv::Affine3<T>::cast() const
joeverbout 0:ea44dc9ed014 440 {
joeverbout 0:ea44dc9ed014 441 return Affine3<Y>(matrix);
joeverbout 0:ea44dc9ed014 442 }
joeverbout 0:ea44dc9ed014 443
joeverbout 0:ea44dc9ed014 444 template<typename T> inline
joeverbout 0:ea44dc9ed014 445 cv::Affine3<T> cv::operator*(const cv::Affine3<T>& affine1, const cv::Affine3<T>& affine2)
joeverbout 0:ea44dc9ed014 446 {
joeverbout 0:ea44dc9ed014 447 return affine2.concatenate(affine1);
joeverbout 0:ea44dc9ed014 448 }
joeverbout 0:ea44dc9ed014 449
joeverbout 0:ea44dc9ed014 450 template<typename T, typename V> inline
joeverbout 0:ea44dc9ed014 451 V cv::operator*(const cv::Affine3<T>& affine, const V& v)
joeverbout 0:ea44dc9ed014 452 {
joeverbout 0:ea44dc9ed014 453 const typename Affine3<T>::Mat4& m = affine.matrix;
joeverbout 0:ea44dc9ed014 454
joeverbout 0:ea44dc9ed014 455 V r;
joeverbout 0:ea44dc9ed014 456 r.x = m.val[0] * v.x + m.val[1] * v.y + m.val[ 2] * v.z + m.val[ 3];
joeverbout 0:ea44dc9ed014 457 r.y = m.val[4] * v.x + m.val[5] * v.y + m.val[ 6] * v.z + m.val[ 7];
joeverbout 0:ea44dc9ed014 458 r.z = m.val[8] * v.x + m.val[9] * v.y + m.val[10] * v.z + m.val[11];
joeverbout 0:ea44dc9ed014 459 return r;
joeverbout 0:ea44dc9ed014 460 }
joeverbout 0:ea44dc9ed014 461
joeverbout 0:ea44dc9ed014 462 static inline
joeverbout 0:ea44dc9ed014 463 cv::Vec3f cv::operator*(const cv::Affine3f& affine, const cv::Vec3f& v)
joeverbout 0:ea44dc9ed014 464 {
joeverbout 0:ea44dc9ed014 465 const cv::Matx44f& m = affine.matrix;
joeverbout 0:ea44dc9ed014 466 cv::Vec3f r;
joeverbout 0:ea44dc9ed014 467 r.val[0] = m.val[0] * v[0] + m.val[1] * v[1] + m.val[ 2] * v[2] + m.val[ 3];
joeverbout 0:ea44dc9ed014 468 r.val[1] = m.val[4] * v[0] + m.val[5] * v[1] + m.val[ 6] * v[2] + m.val[ 7];
joeverbout 0:ea44dc9ed014 469 r.val[2] = m.val[8] * v[0] + m.val[9] * v[1] + m.val[10] * v[2] + m.val[11];
joeverbout 0:ea44dc9ed014 470 return r;
joeverbout 0:ea44dc9ed014 471 }
joeverbout 0:ea44dc9ed014 472
joeverbout 0:ea44dc9ed014 473 static inline
joeverbout 0:ea44dc9ed014 474 cv::Vec3d cv::operator*(const cv::Affine3d& affine, const cv::Vec3d& v)
joeverbout 0:ea44dc9ed014 475 {
joeverbout 0:ea44dc9ed014 476 const cv::Matx44d& m = affine.matrix;
joeverbout 0:ea44dc9ed014 477 cv::Vec3d r;
joeverbout 0:ea44dc9ed014 478 r.val[0] = m.val[0] * v[0] + m.val[1] * v[1] + m.val[ 2] * v[2] + m.val[ 3];
joeverbout 0:ea44dc9ed014 479 r.val[1] = m.val[4] * v[0] + m.val[5] * v[1] + m.val[ 6] * v[2] + m.val[ 7];
joeverbout 0:ea44dc9ed014 480 r.val[2] = m.val[8] * v[0] + m.val[9] * v[1] + m.val[10] * v[2] + m.val[11];
joeverbout 0:ea44dc9ed014 481 return r;
joeverbout 0:ea44dc9ed014 482 }
joeverbout 0:ea44dc9ed014 483
joeverbout 0:ea44dc9ed014 484
joeverbout 0:ea44dc9ed014 485
joeverbout 0:ea44dc9ed014 486 #if defined EIGEN_WORLD_VERSION && defined EIGEN_GEOMETRY_MODULE_H
joeverbout 0:ea44dc9ed014 487
joeverbout 0:ea44dc9ed014 488 template<typename T> inline
joeverbout 0:ea44dc9ed014 489 cv::Affine3<T>::Affine3(const Eigen::Transform<T, 3, Eigen::Affine, (Eigen::RowMajor)>& affine)
joeverbout 0:ea44dc9ed014 490 {
joeverbout 0:ea44dc9ed014 491 cv::Mat(4, 4, cv::DataType<T>::type, affine.matrix().data()).copyTo(matrix);
joeverbout 0:ea44dc9ed014 492 }
joeverbout 0:ea44dc9ed014 493
joeverbout 0:ea44dc9ed014 494 template<typename T> inline
joeverbout 0:ea44dc9ed014 495 cv::Affine3<T>::Affine3(const Eigen::Transform<T, 3, Eigen::Affine>& affine)
joeverbout 0:ea44dc9ed014 496 {
joeverbout 0:ea44dc9ed014 497 Eigen::Transform<T, 3, Eigen::Affine, (Eigen::RowMajor)> a = affine;
joeverbout 0:ea44dc9ed014 498 cv::Mat(4, 4, cv::DataType<T>::type, a.matrix().data()).copyTo(matrix);
joeverbout 0:ea44dc9ed014 499 }
joeverbout 0:ea44dc9ed014 500
joeverbout 0:ea44dc9ed014 501 template<typename T> inline
joeverbout 0:ea44dc9ed014 502 cv::Affine3<T>::operator Eigen::Transform<T, 3, Eigen::Affine, (Eigen::RowMajor)>() const
joeverbout 0:ea44dc9ed014 503 {
joeverbout 0:ea44dc9ed014 504 Eigen::Transform<T, 3, Eigen::Affine, (Eigen::RowMajor)> r;
joeverbout 0:ea44dc9ed014 505 cv::Mat hdr(4, 4, cv::DataType<T>::type, r.matrix().data());
joeverbout 0:ea44dc9ed014 506 cv::Mat(matrix, false).copyTo(hdr);
joeverbout 0:ea44dc9ed014 507 return r;
joeverbout 0:ea44dc9ed014 508 }
joeverbout 0:ea44dc9ed014 509
joeverbout 0:ea44dc9ed014 510 template<typename T> inline
joeverbout 0:ea44dc9ed014 511 cv::Affine3<T>::operator Eigen::Transform<T, 3, Eigen::Affine>() const
joeverbout 0:ea44dc9ed014 512 {
joeverbout 0:ea44dc9ed014 513 return this->operator Eigen::Transform<T, 3, Eigen::Affine, (Eigen::RowMajor)>();
joeverbout 0:ea44dc9ed014 514 }
joeverbout 0:ea44dc9ed014 515
joeverbout 0:ea44dc9ed014 516 #endif /* defined EIGEN_WORLD_VERSION && defined EIGEN_GEOMETRY_MODULE_H */
joeverbout 0:ea44dc9ed014 517
joeverbout 0:ea44dc9ed014 518 //! @endcond
joeverbout 0:ea44dc9ed014 519
joeverbout 0:ea44dc9ed014 520 #endif /* __cplusplus */
joeverbout 0:ea44dc9ed014 521
joeverbout 0:ea44dc9ed014 522 #endif /* __OPENCV_CORE_AFFINE3_HPP__ */
joeverbout 0:ea44dc9ed014 523