// // 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) 2014, OpenCV Foundation, 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. // // Author: Tolga Birdal <tbirdal AT gmail.com> #ifndef __OPENCV_SURFACE_MATCHING_UTILS_HPP_ #define __OPENCV_SURFACE_MATCHING_UTILS_HPP_ #include <cmath> #include <cstdio> namespace cv { namespace ppf_match_3d { const float EPS = 1.192092896e-07F; /* smallest such that 1.0+FLT_EPSILON != 1.0 */ #ifndef M_PI #define M_PI 3.1415926535897932384626433832795 #endif static inline void TNormalize3(Vec3d& v) { double norm = cv::norm(v); if (norm > EPS) { v *= 1.0 / norm; } } /** * \brief Calculate angle between two normalized vectors * * \param [in] a normalized vector * \param [in] b normalized vector * \return angle between a and b vectors in radians */ static inline double TAngle3Normalized(const Vec3d& a, const Vec3d& b) { /* angle = atan2(a dot b, |a x b|) # Bertram (accidental mistake) angle = atan2(|a x b|, a dot b) # Tolga Birdal (correction) angle = acos(a dot b) # Hamdi Sahloul (simplification, a & b are normalized) */ return acos(a.dot(b)); } static inline void rtToPose(const Matx33d& R, const Vec3d& t, Matx44d& Pose) { Matx34d P; hconcat(R, t, P); vconcat(P, Matx14d(0, 0, 0, 1), Pose); } static inline void poseToR(const Matx44d& Pose, Matx33d& R) { Mat(Pose).rowRange(0, 3).colRange(0, 3).copyTo(R); } static inline void poseToRT(const Matx44d& Pose, Matx33d& R, Vec3d& t) { poseToR(Pose, R); Mat(Pose).rowRange(0, 3).colRange(3, 4).copyTo(t); } /** * \brief Axis angle to rotation */ static inline void aaToR(const Vec3d& axis, double angle, Matx33d& R) { const double sinA = sin(angle); const double cosA = cos(angle); const double cos1A = (1 - cosA); uint i, j; Mat(cosA * Matx33d::eye()).copyTo(R); for (i = 0; i < 3; i++) for (j = 0; j < 3; j++) { if (i != j) { // Symmetry skew matrix R(i, j) += (((i + 1) % 3 == j) ? -1 : 1) * sinA * axis[3 - i - j]; } R(i, j) += cos1A * axis[i] * axis[j]; } } /** * \brief Compute a rotation in order to rotate around X direction */ static inline void getUnitXRotation(double angle, Matx33d& Rx) { const double sx = sin(angle); const double cx = cos(angle); Mat(Rx.eye()).copyTo(Rx); Rx(1, 1) = cx; Rx(1, 2) = -sx; Rx(2, 1) = sx; Rx(2, 2) = cx; } /** * \brief Compute a rotation in order to rotate around Y direction */ static inline void getUnitYRotation(double angle, Matx33d& Ry) { const double sy = sin(angle); const double cy = cos(angle); Mat(Ry.eye()).copyTo(Ry); Ry(0, 0) = cy; Ry(0, 2) = sy; Ry(2, 0) = -sy; Ry(2, 2) = cy; } /** * \brief Compute a rotation in order to rotate around Z direction */ static inline void getUnitZRotation(double angle, Matx33d& Rz) { const double sz = sin(angle); const double cz = cos(angle); Mat(Rz.eye()).copyTo(Rz); Rz(0, 0) = cz; Rz(0, 1) = -sz; Rz(1, 0) = sz; Rz(1, 1) = cz; } /** * \brief Convert euler representation to rotation matrix * * \param [in] euler RPY angles * \param [out] R 3x3 Rotation matrix */ static inline void eulerToDCM(const Vec3d& euler, Matx33d& R) { Matx33d Rx, Ry, Rz; getUnitXRotation(euler[0], Rx); getUnitYRotation(euler[1], Ry); getUnitZRotation(euler[2], Rz); Mat(Rx * (Ry * Rz)).copyTo(R); } /** * \brief Compute the transformation needed to rotate n1 onto x axis and p1 to origin */ static inline void computeTransformRT(const Vec3d& p1, const Vec3d& n1, Matx33d& R, Vec3d& t) { // dot product with x axis double angle = acos(n1[0]); // cross product with x axis Vec3d axis(0, n1[2], -n1[1]); // we try to project on the ground plane but it's already parallel if (n1[1] == 0 && n1[2] == 0) { axis[1] = 1; axis[2] = 0; } else { TNormalize3(axis); } aaToR(axis, angle, R); t = -R * p1; } /** * \brief Flip a normal to the viewing direction * * \param [in] point Scene point * \param [in] vp view direction * \param [in] n normal */ static inline void flipNormalViewpoint(const Vec3f& point, const Vec3f& vp, Vec3f& n) { float cos_theta; // See if we need to flip any plane normals Vec3f diff = vp - point; // Dot product between the (viewpoint - point) and the plane normal cos_theta = diff.dot(n); // Flip the plane normal if (cos_theta < 0) { n *= -1; } } /** * \brief Convert a rotation matrix to axis angle representation * * \param [in] R Rotation matrix * \param [out] axis Axis vector * \param [out] angle Angle in radians */ static inline void dcmToAA(Matx33d& R, Vec3d& axis, double *angle) { Mat(Vec3d(R(2, 1) - R(2, 1), R(0, 2) - R(2, 0), R(1, 0) - R(0, 1))).copyTo(axis); TNormalize3(axis); *angle = acos(0.5 * (cv::trace(R) - 1.0)); } /** * \brief Convert axis angle representation to rotation matrix * * \param [in] axis Axis Vector * \param [in] angle Angle (In radians) * \param [out] R 3x3 Rotation matrix */ static inline void aaToDCM(const Vec3d& axis, double angle, Matx33d& R) { uint i, j; Matx33d n = Matx33d::all(0); for (i = 0; i < 3; i++) for (j = 0; j < 3; j++) if (i != j) n(i, j) = (((i + 1) % 3 == j) ? -1 : 1) * axis[3 - i - j]; Mat(Matx33d::eye() + sin(angle) * n + cos(angle) * n * n).copyTo(R); } /** * \brief Convert a discrete cosine matrix to quaternion * * \param [in] R Rotation Matrix * \param [in] q Quaternion */ static inline void dcmToQuat(Matx33d& R, Vec4d& q) { double tr = cv::trace(R); Vec3d v(R(0, 0), R(1, 1), R(2, 2)); int idx = tr > 0.0 ? 3 : (int)(std::max_element(v.val, v.val + 3) - v.val); double norm4 = q[(idx + 1) % 4] = 1.0 + (tr > 0.0 ? tr : 2 * R(idx, idx) - tr); int i, prev, next, step = idx % 2 ? 1 : -1, curr = 3; for (i = 0; i < 3; i++) { curr = (curr + step) % 4; next = (curr + 1) % 3, prev = (curr + 2) % 3; q[(idx + i + 2) % 4] = R(next, prev) + (tr > 0.0 || idx == curr ? -1 : 1) * R(prev, next); } q *= 0.5 / sqrt(norm4); } /** * \brief Convert quaternion to a discrete cosine matrix * * \param [in] q Quaternion (w is at first element) * \param [in] R Rotation Matrix * */ static inline void quatToDCM(Vec4d& q, Matx33d& R) { Vec4d sq = q.mul(q); double tmp1, tmp2; R(0, 0) = sq[0] + sq[1] - sq[2] - sq[3]; // since norm(q) = 1 R(1, 1) = sq[0] - sq[1] + sq[2] - sq[3]; R(2, 2) = sq[0] - sq[1] - sq[2] + sq[3]; tmp1 = q[1] * q[2]; tmp2 = q[3] * q[0]; R(0, 1) = 2.0 * (tmp1 + tmp2); R(1, 0) = 2.0 * (tmp1 - tmp2); tmp1 = q[1] * q[3]; tmp2 = q[2] * q[0]; R(0, 2) = 2.0 * (tmp1 - tmp2); R(2, 0) = 2.0 * (tmp1 + tmp2); tmp1 = q[2] * q[3]; tmp2 = q[1] * q[0]; R(1, 2) = 2.0 * (tmp1 + tmp2); R(2, 1) = 2.0 * (tmp1 - tmp2); } } // namespace ppf_match_3d } // namespace cv #endif