Renesas / opencv-lib

openCV library for Renesas RZ/A

Dependents:   RZ_A2M_Mbed_samples

include/opencv2/core/affine.hpp@0:0e0631af0305, 2021-01-29 (annotated)

Committer:
RyoheiHagimoto
Date:
Fri Jan 29 04:53:38 2021 +0000
Revision:
0:0e0631af0305
copied from https://github.com/d-kato/opencv-lib.

Who changed what in which revision?

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