From ceadaed108b560a4276699003be679181e51e3be Mon Sep 17 00:00:00 2001 From: Anatoly Baksheev Date: Sun, 17 Mar 2013 00:40:35 +0400 Subject: [PATCH] Added Affine3 class --- modules/calib3d/test/test_affine3.cpp | 81 ++++ modules/core/include/opencv2/core/affine.hpp | 386 +++++++++++++++++++ 2 files changed, 467 insertions(+) create mode 100644 modules/calib3d/test/test_affine3.cpp create mode 100644 modules/core/include/opencv2/core/affine.hpp diff --git a/modules/calib3d/test/test_affine3.cpp b/modules/calib3d/test/test_affine3.cpp new file mode 100644 index 0000000000..196d428e45 --- /dev/null +++ b/modules/calib3d/test/test_affine3.cpp @@ -0,0 +1,81 @@ +/*M/////////////////////////////////////////////////////////////////////////////////////// + // + // IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING. + // + // By downloading, copying, installing or using the software you agree to this license. + // If you do not agree to this license, do not download, install, + // copy or use the software. + // + // + // License Agreement + // For Open Source Computer Vision Library + // + // Copyright (C) 2000-2008, Intel Corporation, all rights reserved. + // Copyright (C) 2008-2013, Willow Garage Inc., all rights reserved. + // Third party copyrights are property of their respective owners. + // + // Redistribution and use in source and binary forms, with or without modification, + // are permitted provided that the following conditions are met: + // + // * Redistribution's of source code must retain the above copyright notice, + // this list of conditions and the following disclaimer. + // + // * Redistribution's in binary form must reproduce the above copyright notice, + // this list of conditions and the following disclaimer in the documentation + // and / or other materials provided with the distribution. + // + // * The name of the copyright holders may not be used to endorse or promote products + // derived from this software without specific prior written permission. + // + // This software is provided by the copyright holders and contributors "as is" and + // any express or implied warranties, including, but not limited to, the implied + // warranties of merchantability and fitness for a particular purpose are disclaimed. + // In no event shall the Intel Corporation or contributors be liable for any direct, + // indirect, incidental, special, exemplary, or consequential damages + // (including, but not limited to, procurement of substitute goods or services; + // loss of use, data, or profits; or business interruption) however caused + // and on any theory of liability, whether in contract, strict liability, + // or tort (including negligence or otherwise) arising in any way out of + // the use of this software, even if advised of the possibility of such damage. + // + //M*/ + +#include "test_precomp.hpp" +#include "opencv2/core/affine.hpp" +#include "opencv2/calib3d.hpp" +#include + +TEST(Calib3d_Affine3f, accuracy) +{ + cv::Vec3d rvec(0.2, 0.5, 0.3); + cv::Affine3d affine(rvec); + + cv::Mat expected; + cv::Rodrigues(rvec, expected); + + + ASSERT_EQ(0, norm(cv::Mat(affine.matrix, false).colRange(0, 3).rowRange(0, 3) != expected)); + ASSERT_EQ(0, norm(cv::Mat(affine.linear()) != expected)); + + + cv::Matx33d R = cv::Matx33d::eye(); + + double angle = 50; + R.val[0] = R.val[4] = std::cos(CV_PI*angle/180.0); + R.val[3] = std::sin(CV_PI*angle/180.0); + R.val[1] = -R.val[3]; + + + cv::Affine3d affine1(cv::Mat(cv::Vec3d(0.2, 0.5, 0.3)).reshape(1, 1), cv::Vec3d(4, 5, 6)); + cv::Affine3d affine2(R, cv::Vec3d(1, 1, 0.4)); + + cv::Affine3d result = affine1.inv() * affine2; + + expected = cv::Mat(affine1.matrix.inv(cv::DECOMP_SVD)) * cv::Mat(affine2.matrix, false); + + + cv::Mat diff; + cv::absdiff(expected, result.matrix, diff); + + ASSERT_LT(cv::norm(diff, cv::NORM_INF), 1e-15); +} diff --git a/modules/core/include/opencv2/core/affine.hpp b/modules/core/include/opencv2/core/affine.hpp new file mode 100644 index 0000000000..ccbc5c81b3 --- /dev/null +++ b/modules/core/include/opencv2/core/affine.hpp @@ -0,0 +1,386 @@ +/*M/////////////////////////////////////////////////////////////////////////////////////// + // + // IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING. + // + // By downloading, copying, installing or using the software you agree to this license. + // If you do not agree to this license, do not download, install, + // copy or use the software. + // + // + // License Agreement + // For Open Source Computer Vision Library + // + // Copyright (C) 2000-2008, Intel Corporation, all rights reserved. + // Copyright (C) 2008-2013, Willow Garage Inc., all rights reserved. + // Third party copyrights are property of their respective owners. + // + // Redistribution and use in source and binary forms, with or without modification, + // are permitted provided that the following conditions are met: + // + // * Redistribution's of source code must retain the above copyright notice, + // this list of conditions and the following disclaimer. + // + // * Redistribution's in binary form must reproduce the above copyright notice, + // this list of conditions and the following disclaimer in the documentation + // and / or other materials provided with the distribution. + // + // * The name of the copyright holders may not be used to endorse or promote products + // derived from this software without specific prior written permission. + // + // This software is provided by the copyright holders and contributors "as is" and + // any express or implied warranties, including, but not limited to, the implied + // warranties of merchantability and fitness for a particular purpose are disclaimed. + // In no event shall the Intel Corporation or contributors be liable for any direct, + // indirect, incidental, special, exemplary, or consequential damages + // (including, but not limited to, procurement of substitute goods or services; + // loss of use, data, or profits; or business interruption) however caused + // and on any theory of liability, whether in contract, strict liability, + // or tort (including negligence or otherwise) arising in any way out of + // the use of this software, even if advised of the possibility of such damage. + // + //M*/ + +#ifndef __OPENCV_CORE_AFFINE3_HPP__ +#define __OPENCV_CORE_AFFINE3_HPP__ + +#ifdef __cplusplus + +#include + +namespace cv +{ + template + class CV_EXPORTS Affine3 + { + public: + typedef T float_type; + typedef cv::Matx Mat3; + typedef cv::Matx Mat4; + typedef cv::Vec Vec3; + + Affine3(); + + //Augmented affine matrix + Affine3(const Mat4& affine); + + //Rotation matrix + Affine3(const Mat3& R, const Vec3& t = Vec3::all(0)); + + //Rodrigues vector + Affine3(const Vec3& rvec, const Vec3& t = Vec3::all(0)); + + //Combines all contructors above. Supports 4x4, 3x3, 1x3, 3x1 sizes of data matrix + explicit Affine3(const cv::Mat& data, const Vec3& t = Vec3::all(0)); + + //Euler angles + Affine3(float_type alpha, float_type beta, float_type gamma, const Vec3& t = Vec3::all(0)); + + static Affine3 Identity(); + + //Rotation matrix + void rotation(const Mat3& R); + + //Rodrigues vector + void rotation(const Vec3& rvec); + + //Combines rotation methods above. Suports 3x3, 1x3, 3x1 sizes of data matrix; + void rotation(const Mat& data); + + //Euler angles + void rotation(float_type alpha, float_type beta, float_type gamma); + + void linear(const Mat3& L); + void translation(const Vec3& t); + + Mat3 rotation() const; + Mat3 linear() const; + Vec3 translation() const; + + Affine3 inv(int method = cv::DECOMP_SVD) const; + + // a.rotate(R) is equivalent to Affine(R, 0) * a; + Affine3 rotate(const Mat3& R) const; + + // a.translate(t) is equivalent to Affine(E, t) * a; + Affine3 translate(const Vec3& t) const; + + // a.concatenate(affine) is equivalent to affine * a; + Affine3 concatenate(const Affine3& affine) const; + + template operator Affine3() const; + + Mat4 matrix; + +#if defined EIGEN_WORLD_VERSION && defined EIGEN_GEOMETRY_MODULE_H + Affine3(const Eigen::Transform& affine); + Affine3(const Eigen::Transform& affine); + operator Eigen::Transform() const; + operator Eigen::Transform() const; +#endif + }; + + template Affine3 operator*(const Affine3& affine1, const Affine3& affine2); + template V operator*(const Affine3& affine, const V& vector); + + typedef Affine3 Affine3f; + typedef Affine3 Affine3d; + + cv::Vec3f operator*(const cv::Affine3f& affine, const cv::Vec3f& vector); + cv::Vec3d operator*(const cv::Affine3d& affine, const cv::Vec3d& vector); +} + + +/////////////////////////////////////////////////////////////////////////////////// +/// Implementaiton + +template inline cv::Affine3::Affine3() : matrix(Mat4::eye()) {} +template inline cv::Affine3::Affine3(const Mat4& affine) : matrix(affine) {} + +template inline cv::Affine3::Affine3(const Mat3& R, const Vec3& t) +{ + rotation(R); + translation(t); + matrix.val[12] = matrix.val[13] = matrix.val[14] = 0; + matrix.val[15] = 1; +} + +template inline cv::Affine3::Affine3(const Vec3& rvec, const Vec3& t) +{ + rotation(rvec); + translation(t); + matrix.val[12] = matrix.val[13] = matrix.val[14] = 0; + matrix.val[15] = 1; +} + +template inline cv::Affine3::Affine3(const cv::Mat& data, const Vec3& t) +{ + CV_Assert(data.type() == cv::DataType::type); + + if (data.cols == 4 && data.rows == 4) + { + data.copyTo(matrix); + return; + } + + rotation(data); + translation(t); + matrix.val[12] = matrix.val[13] = matrix.val[14] = 0; + matrix.val[15] = 1; +} + +template inline cv::Affine3::Affine3(float_type alpha, float_type beta, float_type gamma, const Vec3& t) +{ + rotation(alpha, beta, gamma); + translation(t); + matrix.val[12] = matrix.val[13] = matrix.val[14] = 0; + matrix.val[15] = 1; +} + +template inline cv::Affine3 cv::Affine3::Identity() +{ + return Affine3(cv::Affine3::Mat4::eye()); +} + +template inline void cv::Affine3::rotation(const Mat3& R) { linear(R); } + +template inline void cv::Affine3::rotation(const Vec3& rvec) +{ + double rx = rvec[0], ry = rvec[1], rz = rvec[2]; + double theta = std::sqrt(rx*rx + ry*ry + rz*rz); + + if (theta < DBL_EPSILON) + rotation(Mat3::eye()); + else + { + const double I[] = { 1, 0, 0, 0, 1, 0, 0, 0, 1 }; + + double c = std::cos(theta); + double s = std::sin(theta); + double c1 = 1. - c; + double itheta = theta ? 1./theta : 0.; + + rx *= itheta; ry *= itheta; rz *= itheta; + + double rrt[] = { rx*rx, rx*ry, rx*rz, rx*ry, ry*ry, ry*rz, rx*rz, ry*rz, rz*rz }; + double _r_x_[] = { 0, -rz, ry, rz, 0, -rx, -ry, rx, 0 }; + Mat3 R; + + // R = cos(theta)*I + (1 - cos(theta))*r*rT + sin(theta)*[r_x] + // where [r_x] is [0 -rz ry; rz 0 -rx; -ry rx 0] + for(int k = 0; k < 9; ++k) + R.val[k] = static_cast(c*I[k] + c1*rrt[k] + s*_r_x_[k]); + + rotation(R); + } +} + +//Combines rotation methods above. Suports 3x3, 1x3, 3x1 sizes of data matrix; +template inline void cv::Affine3::rotation(const cv::Mat& data) +{ + CV_Assert(data.type() == cv::DataType::type); + + if (data.cols == 3 && data.rows == 3) + { + Mat3 R; + data.copyTo(R); + rotation(R); + } + else if ((data.cols == 3 && data.rows == 1) || (data.cols == 1 && data.rows == 3)) + { + Vec3 rvec; + data.reshape(1, 3).copyTo(rvec); + rotation(rvec); + } + else + CV_Assert(!"Input marix can be 3x3, 1x3 or 3x1"); +} + +template inline void cv::Affine3::rotation(float_type alpha, float_type beta, float_type gamma) +{ + rotation(Vec3(alpha, beta, gamma)); +} + +template inline void cv::Affine3::linear(const Mat3& L) +{ + matrix.val[0] = L.val[0]; matrix.val[1] = L.val[1]; matrix.val[ 2] = L.val[2]; + matrix.val[4] = L.val[3]; matrix.val[5] = L.val[4]; matrix.val[ 6] = L.val[5]; + matrix.val[8] = L.val[6]; matrix.val[9] = L.val[7]; matrix.val[10] = L.val[8]; +} + +template inline void cv::Affine3::translation(const Vec3& t) +{ + matrix.val[3] = t[0]; matrix.val[7] = t[1]; matrix.val[11] = t[2]; +} + +template inline typename cv::Affine3::Mat3 cv::Affine3::rotation() const { return linear(); } +template inline typename cv::Affine3::Mat3 cv::Affine3::linear() const +{ + cv::Affine3::Mat3 R; + R.val[0] = matrix.val[0]; R.val[1] = matrix.val[1]; R.val[2] = matrix.val[ 2]; + R.val[3] = matrix.val[4]; R.val[4] = matrix.val[5]; R.val[5] = matrix.val[ 6]; + R.val[6] = matrix.val[8]; R.val[7] = matrix.val[9]; R.val[8] = matrix.val[10]; + return R; +} + +template inline typename cv::Affine3::Vec3 cv::Affine3::translation() const +{ + return Vec3(matrix.val[3], matrix.val[7], matrix.val[11]); +} + +template inline cv::Affine3 cv::Affine3::inv(int method) const +{ + return matrix.inv(method); +} + +template inline cv::Affine3 cv::Affine3::rotate(const Mat3& R) const +{ + Mat3 Lc = linear(); + Vec3 tc = translation(); + Mat4 result; + result.val[12] = result.val[13] = result.val[14] = 0; + result.val[15] = 1; + + for(int j = 0; j < 3; ++j) + { + for(int i = 0; i < 3; ++i) + { + float_type value = 0; + for(int k = 0; k < 3; ++k) + value += R(j, k) * Lc(k, i); + result(j, i) = value; + } + + result(j, 3) = R.row(j).dot(tc.t()); + } + return result; +} + +template inline cv::Affine3 cv::Affine3::translate(const Vec3& t) const +{ + Mat4 m = matrix; + m.val[ 3] += t[0]; + m.val[ 7] += t[1]; + m.val[11] += t[2]; + return m; +} + +template inline cv::Affine3 cv::Affine3::concatenate(const Affine3& affine) const +{ + return (*this).rotate(affine.rotation()).translate(affine.translation()); +} + +template template inline cv::Affine3::operator Affine3() const +{ + return Affine3(matrix); +} + +template inline cv::Affine3 cv::operator*(const cv::Affine3& affine1, const cv::Affine3& affine2) +{ + return affine2.concatenate(affine1); +} + +template inline V cv::operator*(const cv::Affine3& affine, const V& v) +{ + const typename Affine3::Mat4& m = affine.matrix; + + V r; + r.x = m.val[0] * v.x + m.val[1] * v.y + m.val[ 2] * v.z + m.val[ 3]; + r.y = m.val[4] * v.x + m.val[5] * v.y + m.val[ 6] * v.z + m.val[ 7]; + r.z = m.val[8] * v.x + m.val[9] * v.y + m.val[10] * v.z + m.val[11]; + return r; +} + +inline cv::Vec3f cv::operator*(const cv::Affine3f& affine, const cv::Vec3f& v) +{ + const cv::Matx44f& m = affine.matrix; + cv::Vec3f r; + r.val[0] = m.val[0] * v[0] + m.val[1] * v[1] + m.val[ 2] * v[2] + m.val[ 3]; + r.val[1] = m.val[4] * v[0] + m.val[5] * v[1] + m.val[ 6] * v[2] + m.val[ 7]; + r.val[2] = m.val[8] * v[0] + m.val[9] * v[1] + m.val[10] * v[2] + m.val[11]; + return r; +} + +inline cv::Vec3d cv::operator*(const cv::Affine3d& affine, const cv::Vec3d& v) +{ + const cv::Matx44d& m = affine.matrix; + cv::Vec3d r; + r.val[0] = m.val[0] * v[0] + m.val[1] * v[1] + m.val[ 2] * v[2] + m.val[ 3]; + r.val[1] = m.val[4] * v[0] + m.val[5] * v[1] + m.val[ 6] * v[2] + m.val[ 7]; + r.val[2] = m.val[8] * v[0] + m.val[9] * v[1] + m.val[10] * v[2] + m.val[11]; + return r; +} + +#if defined EIGEN_WORLD_VERSION && defined EIGEN_GEOMETRY_MODULE_H + +template inline cv::Affine3::Affine3(const Eigen::Transform& affine) +{ + cv::Mat(4, 4, cv::DataType::type, affine.matrix().data()).copyTo(matrix); +} + +template inline cv::Affine3::Affine3(const Eigen::Transform& affine) +{ + Eigen::Transform a = affine; + cv::Mat(4, 4, cv::DataType::type, a.matrix().data()).copyTo(matrix); +} + +template inline cv::Affine3::operator Eigen::Transform() const +{ + Eigen::Transform r; + cv::Mat hdr(4, 4, cv::DataType::type, r.matrix().data()); + cv::Mat(matrix, false).copyTo(hdr); + return r; +} + +template inline cv::Affine3::operator Eigen::Transform() const +{ + return this->operator Eigen::Transform(); +} + +#endif /* defined EIGEN_WORLD_VERSION && defined EIGEN_GEOMETRY_MODULE_H */ + + +#endif /* __cplusplus */ + +#endif /* __OPENCV_CORE_AFFINE3_HPP__ */ + +