From e6420bde73f57cdbf1024d0b39049f22df3d9414 Mon Sep 17 00:00:00 2001 From: Samson Yilma Date: Sat, 26 Apr 2014 20:13:27 -0400 Subject: [PATCH] Added function decomposeHomographyMat. New files added are homography_decomp.cpp and test_homography_decomp.cpp. Modified files calib3d.hpp and camera_calibration_and_3d_reconstruction.rst. --- ...mera_calibration_and_3d_reconstruction.rst | 23 + modules/calib3d/include/opencv2/calib3d.hpp | 5 + modules/calib3d/src/homography_decomp.cpp | 480 ++++++++++++++++++ .../calib3d/test/test_homography_decomp.cpp | 138 +++++ 4 files changed, 646 insertions(+) create mode 100644 modules/calib3d/src/homography_decomp.cpp create mode 100644 modules/calib3d/test/test_homography_decomp.cpp diff --git a/modules/calib3d/doc/camera_calibration_and_3d_reconstruction.rst b/modules/calib3d/doc/camera_calibration_and_3d_reconstruction.rst index 36af8362f..0170904a6 100644 --- a/modules/calib3d/doc/camera_calibration_and_3d_reconstruction.rst +++ b/modules/calib3d/doc/camera_calibration_and_3d_reconstruction.rst @@ -758,6 +758,27 @@ They are :math:`[R_2, -t]`. By decomposing ``E``, you can only get the direction of the translation, so the function returns unit ``t``. +decomposeHomographyMat +-------------------------- +Decompose a homography matrix to rotation(s), translation(s) and plane normal(s). + +.. ocv:function:: int decomposeHomographyMat( InputArray H, InputArray K, OutputArrayOfArrays rotations, OutputArrayOfArrays translations, OutputArrayOfArrays normals) + + :param H: The input homography matrix. + + :param K: The input intrinsic camera calibration matrix. + + :param rotations: Array of rotation matrices. + + :param translations: Array of translation matrices. + + :param normals: Array of plane normal matrices. + +This function extracts relative camera motion between two views observing a planar object from the homography ``H`` induced by the plane. +The intrinsic camera matrix ``K`` must also be provided. The function may return up to four mathematical solution sets. At least two of the +solutions may further be invalidated if point correspondences are available by applying positive depth constraint (all points must be in front of the camera). +The decomposition method is described in detail in [Malis]_. + recoverPose --------------- @@ -1512,3 +1533,5 @@ The function reconstructs 3-dimensional points (in homogeneous coordinates) by u .. [Slabaugh] Slabaugh, G.G. Computing Euler angles from a rotation matrix. http://www.soi.city.ac.uk/~sbbh653/publications/euler.pdf (verified: 2013-04-15) .. [Zhang2000] Z. Zhang. A Flexible New Technique for Camera Calibration. IEEE Transactions on Pattern Analysis and Machine Intelligence, 22(11):1330-1334, 2000. + +.. [Malis] Malis, E. and Vargas, M. Deeper understanding of the homography decomposition for vision-based control, Research Report 6303, INRIA (2007) diff --git a/modules/calib3d/include/opencv2/calib3d.hpp b/modules/calib3d/include/opencv2/calib3d.hpp index 8b9b69c3a..fd6ef8cee 100644 --- a/modules/calib3d/include/opencv2/calib3d.hpp +++ b/modules/calib3d/include/opencv2/calib3d.hpp @@ -314,6 +314,11 @@ CV_EXPORTS_W int estimateAffine3D(InputArray src, InputArray dst, double ransacThreshold = 3, double confidence = 0.99); +CV_EXPORTS_W int decomposeHomographyMat(InputArray _H, + InputArray _K, + OutputArrayOfArrays _rotations, + OutputArrayOfArrays _translations, + OutputArrayOfArrays _normals); class CV_EXPORTS_W StereoMatcher : public Algorithm { diff --git a/modules/calib3d/src/homography_decomp.cpp b/modules/calib3d/src/homography_decomp.cpp new file mode 100644 index 000000000..8323453cd --- /dev/null +++ b/modules/calib3d/src/homography_decomp.cpp @@ -0,0 +1,480 @@ +/*M/////////////////////////////////////////////////////////////////////////////////////// + // + // This is a homography decomposition implementation contributed to OpenCV + // by Samson Yilma. It implements the homography decomposition algorithm + // descriped in the research report: + // Malis, E and Vargas, M, "Deeper understanding of the homography decomposition + // for vision-based control", Research Report 6303, INRIA (2007) + // + // 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, Samson Yilma¸ (samson_yilma@yahoo.com), 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 "precomp.hpp" + +namespace cv +{ + +namespace HomographyDecomposition +{ + +//struct to hold solutions of homography decomposition +typedef struct _CameraMotion { + cv::Matx33d R; //!< rotation matrix + cv::Vec3d n; //!< normal of the plane the camera is looking at + cv::Vec3d t; //!< translation vector +} CameraMotion; + +inline int signd(const double x) +{ + return ( x >= 0 ? 1 : -1 ); +} + +class HomographyDecomp { + +public: + HomographyDecomp() {} + virtual ~HomographyDecomp() {} + virtual void decomposeHomography(const cv::Matx33d& H, const cv::Matx33d& K, + std::vector& camMotions); + bool isRotationValid(const cv::Matx33d& R, const double epsilon=0.01); + +protected: + bool passesSameSideOfPlaneConstraint(CameraMotion& motion); + virtual void decompose(std::vector& camMotions) = 0; + const cv::Matx33d& getHnorm() const { + return _Hnorm; + } + +private: + cv::Matx33d normalize(const cv::Matx33d& H, const cv::Matx33d& K); + void removeScale(); + cv::Matx33d _Hnorm; +}; + +class HomographyDecompZhang : public HomographyDecomp { + +public: + HomographyDecompZhang():HomographyDecomp() {} + virtual ~HomographyDecompZhang() {} + +private: + virtual void decompose(std::vector& camMotions); + bool findMotionFrom_tstar_n(const cv::Vec3d& tstar, const cv::Vec3d& n, CameraMotion& motion); +}; + +class HomographyDecompInria : public HomographyDecomp { + +public: + HomographyDecompInria():HomographyDecomp() {} + virtual ~HomographyDecompInria() {} + +private: + virtual void decompose(std::vector& camMotions); + double oppositeOfMinor(const cv::Matx33d& M, const int row, const int col); + void findRmatFrom_tstar_n(const cv::Vec3d& tstar, const cv::Vec3d& n, const double v, cv::Matx33d& R); +}; + +// normalizes homography with intrinsic camera parameters +Matx33d HomographyDecomp::normalize(const Matx33d& H, const Matx33d& K) +{ + return K.inv() * H * K; +} + +void HomographyDecomp::removeScale() +{ + Mat W; + SVD::compute(_Hnorm, W); + _Hnorm = _Hnorm * (1.0/W.at(1)); +} + +/*! This checks that the input is a pure rotation matrix 'm'. + * The conditions for this are: R' * R = I and det(R) = 1 (proper rotation matrix) + */ +bool HomographyDecomp::isRotationValid(const Matx33d& R, const double epsilon) +{ + Matx33d RtR = R.t() * R; + Matx33d I(1,0,0, 0,1,0, 0,0,1); + if (norm(RtR, I, NORM_INF) > epsilon) + return false; + return (fabs(determinant(R) - 1.0) < epsilon); +} + +bool HomographyDecomp::passesSameSideOfPlaneConstraint(CameraMotion& motion) +{ + typedef Matx Matx11d; + Matx31d t = Matx31d(motion.t); + Matx31d n = Matx31d(motion.n); + Matx11d proj = n.t() * motion.R.t() * t; + if ( (1 + proj(0, 0) ) <= 0 ) + return false; + return true; +} + +//!main routine to decompose homography +void HomographyDecomp::decomposeHomography(const Matx33d& H, const cv::Matx33d& K, + std::vector& camMotions) +{ + //normalize homography matrix with intrinsic camera matrix + _Hnorm = normalize(H, K); + //remove scale of the normalized homography + removeScale(); + //apply decomposition + decompose(camMotions); +} + +/* function computes R&t from tstar, and plane normal(n) using + R = H * inv(I + tstar*transpose(n) ); + t = R * tstar; + returns true if computed R&t is a valid solution + */ +bool HomographyDecompZhang::findMotionFrom_tstar_n(const cv::Vec3d& tstar, const cv::Vec3d& n, CameraMotion& motion) +{ + Matx31d tstar_m = Mat(tstar); + Matx31d n_m = Mat(n); + Matx33d temp = tstar_m * n_m.t(); + temp(0, 0) += 1.0; + temp(1, 1) += 1.0; + temp(2, 2) += 1.0; + motion.R = getHnorm() * temp.inv(); + motion.t = motion.R * tstar; + motion.n = n; + return passesSameSideOfPlaneConstraint(motion); +} + +void HomographyDecompZhang::decompose(std::vector& camMotions) +{ + Mat W, U, Vt; + SVD::compute(getHnorm(), W, U, Vt); + double lambda1=W.at(0); + double lambda3=W.at(2); + double lambda1m3 = (lambda1-lambda3); + double lambda1m3_2 = lambda1m3*lambda1m3; + double lambda1t3 = lambda1*lambda3; + + double t1 = 1.0/(2.0*lambda1t3); + double t2 = sqrtf(1.0+4.0*lambda1t3/lambda1m3_2); + double t12 = t1*t2; + + double e1 = -t1 + t12; //t1*(-1.0f + t2 ); + double e3 = -t1 - t12; //t1*(-1.0f - t2); + double e1_2 = e1*e1; + double e3_2 = e3*e3; + + double nv1p = sqrtf(e1_2*lambda1m3_2 + 2*e1*(lambda1t3-1) + 1.0); + double nv3p = sqrtf(e3_2*lambda1m3_2 + 2*e3*(lambda1t3-1) + 1.0); + double v1p[3], v3p[3]; + + v1p[0]=Vt.at(0)*nv1p, v1p[1]=Vt.at(1)*nv1p, v1p[2]=Vt.at(2)*nv1p; + v3p[0]=Vt.at(6)*nv3p, v3p[1]=Vt.at(7)*nv3p, v3p[2]=Vt.at(8)*nv3p; + + /*The eight solutions are + (A): tstar = +- (v1p - v3p)/(e1 -e3), n = +- (e1*v3p - e3*v1p)/(e1-e3) + (B): tstar = +- (v1p + v3p)/(e1 -e3), n = +- (e1*v3p + e3*v1p)/(e1-e3) + */ + double v1pmv3p[3], v1ppv3p[3]; + double e1v3me3v1[3], e1v3pe3v1[3]; + double inv_e1me3 = 1.0/(e1-e3); + + for(int kk=0;kk<3;++kk){ + v1pmv3p[kk] = v1p[kk]-v3p[kk]; + v1ppv3p[kk] = v1p[kk]+v3p[kk]; + } + + for(int kk=0; kk<3; ++kk){ + double e1v3 = e1*v3p[kk]; + double e3v1=e3*v1p[kk]; + e1v3me3v1[kk] = e1v3-e3v1; + e1v3pe3v1[kk] = e1v3+e3v1; + } + + Vec3d tstar_p, tstar_n; + Vec3d n_p, n_n; + + ///Solution group A + for(int kk=0; kk<3; ++kk) { + tstar_p[kk] = v1pmv3p[kk]*inv_e1me3; + tstar_n[kk] = -tstar_p[kk]; + n_p[kk] = e1v3me3v1[kk]*inv_e1me3; + n_n[kk] = -n_p[kk]; + } + + CameraMotion cmotion; + //(A) Four different combinations for solution A + // (i) (+, +) + if (findMotionFrom_tstar_n(tstar_p, n_p, cmotion)) + camMotions.push_back(cmotion); + + // (ii) (+, -) + if (findMotionFrom_tstar_n(tstar_p, n_n, cmotion)) + camMotions.push_back(cmotion); + + // (iii) (-, +) + if (findMotionFrom_tstar_n(tstar_n, n_p, cmotion)) + camMotions.push_back(cmotion); + + // (iv) (-, -) + if (findMotionFrom_tstar_n(tstar_n, n_n, cmotion)) + camMotions.push_back(cmotion); + ////////////////////////////////////////////////////////////////// + ///Solution group B + for(int kk=0;kk<3;++kk){ + tstar_p[kk] = v1ppv3p[kk]*inv_e1me3; + tstar_n[kk] = -tstar_p[kk]; + n_p[kk] = e1v3pe3v1[kk]*inv_e1me3; + n_n[kk] = -n_p[kk]; + } + + //(B) Four different combinations for solution B + // (i) (+, +) + if (findMotionFrom_tstar_n(tstar_p, n_p, cmotion)) + camMotions.push_back(cmotion); + + // (ii) (+, -) + if (findMotionFrom_tstar_n(tstar_p, n_n, cmotion)) + camMotions.push_back(cmotion); + + // (iii) (-, +) + if (findMotionFrom_tstar_n(tstar_n, n_p, cmotion)) + camMotions.push_back(cmotion); + + // (iv) (-, -) + if (findMotionFrom_tstar_n(tstar_n, n_n, cmotion)) + camMotions.push_back(cmotion); +} + +double HomographyDecompInria::oppositeOfMinor(const Matx33d& M, const int row, const int col) +{ + int x1 = col == 0 ? 1 : 0; + int x2 = col == 2 ? 1 : 2; + int y1 = row == 0 ? 1 : 0; + int y2 = row == 2 ? 1 : 2; + + return (M(y1, x2) * M(y2, x1) - M(y1, x1) * M(y2, x2)); +} + +//computes R = H( I - (2/v)*te_star*ne_t ) +void HomographyDecompInria::findRmatFrom_tstar_n(const cv::Vec3d& tstar, const cv::Vec3d& n, const double v, cv::Matx33d& R) +{ + Matx31d tstar_m = Matx31d(tstar); + Matx31d n_m = Matx31d(n); + Matx33d I(1.0, 0.0, 0.0, + 0.0, 1.0, 0.0, + 0.0, 0.0, 1.0); + + R = getHnorm() * (I - (2/v) * tstar_m * n_m.t() ); +} + +void HomographyDecompInria::decompose(std::vector& camMotions) +{ + const double epsilon = 0.001; + Matx33d S; + + //S = H'H - I + S = getHnorm().t() * getHnorm(); + S(0, 0) -= 1.0; + S(1, 1) -= 1.0; + S(2, 2) -= 1.0; + + //check if H is rotation matrix + if( norm(S, NORM_INF) < epsilon) { + CameraMotion motion; + motion.R = Matx33d(getHnorm()); + motion.t = Vec3d(0, 0, 0); + motion.n = Vec3d(0, 0, 0); + camMotions.push_back(motion); + return; + } + + //! Compute nvectors + Vec3d npa, npb; + + double M00 = oppositeOfMinor(S, 0, 0); + double M11 = oppositeOfMinor(S, 1, 1); + double M22 = oppositeOfMinor(S, 2, 2); + + double rtM00 = sqrt(M00); + double rtM11 = sqrt(M11); + double rtM22 = sqrt(M22); + + double M01 = oppositeOfMinor(S, 0, 1); + double M12 = oppositeOfMinor(S, 1, 2); + double M02 = oppositeOfMinor(S, 0, 2); + + int e12 = signd(M12); + int e02 = signd(M02); + int e01 = signd(M01); + + double nS00 = abs(S(0, 0)); + double nS11 = abs(S(1, 1)); + double nS22 = abs(S(2, 2)); + + //find max( |Sii| ), i=0, 1, 2 + int indx = 0; + if(nS00 < nS11){ + indx = 1; + if( nS11 < nS22 ) + indx = 2; + } + else { + if(nS00 < nS22 ) + indx = 2; + } + + switch (indx) { + case 0: + npa[0] = S(0, 0), npb[0] = S(0, 0); + npa[1] = S(0, 1) + rtM22, npb[1] = S(0, 1) - rtM22; + npa[2] = S(0, 2) + e12 * rtM11, npb[2] = S(0, 2) - e12 * rtM11; + break; + case 1: + npa[0] = S(0, 1) + rtM22, npb[0] = S(0, 1) - rtM22; + npa[1] = S(1, 1), npb[1] = S(1, 1); + npa[2] = S(1, 2) - e02 * rtM00, npb[2] = S(1, 2) + e02 * rtM00; + break; + case 2: + npa[0] = S(0, 2) + e01 * rtM11, npb[0] = S(0, 2) - e01 * rtM11; + npa[1] = S(1, 2) + rtM00, npb[1] = S(1, 2) - rtM00; + npa[2] = S(2, 2), npb[2] = S(2, 2); + break; + default: + break; + } + + double traceS = S(0, 0) + S(1, 1) + S(2, 2); + double v = 2.0 * sqrtf(1 + traceS - M00 - M11 - M22); + + double ESii = signd(S(indx, indx)) ; + double r_2 = 2 + traceS + v; + double nt_2 = 2 + traceS - v; + + double r = sqrt(r_2); + double n_t = sqrt(nt_2); + + Vec3d na = npa / norm(npa); + Vec3d nb = npb / norm(npb); + + double half_nt = 0.5 * n_t; + double esii_t_r = ESii * r; + + Vec3d ta_star = half_nt * (esii_t_r * nb - n_t * na); + Vec3d tb_star = half_nt * (esii_t_r * na - n_t * nb); + + camMotions.resize(4); + + Matx33d Ra, Rb; + Vec3d ta, tb; + + //Ra, ta, na + findRmatFrom_tstar_n(ta_star, na, v, Ra); + ta = Ra * ta_star; + + camMotions[0].R = Ra; + camMotions[0].t = ta; + camMotions[0].n = na; + + //Ra, -ta, -na + camMotions[1].R = Ra; + camMotions[1].t = -ta; + camMotions[1].n = -na; + + //Rb, tb, nb + findRmatFrom_tstar_n(tb_star, nb, v, Rb); + tb = Rb * tb_star; + + camMotions[2].R = Rb; + camMotions[2].t = tb; + camMotions[2].n = nb; + + //Rb, -tb, -nb + camMotions[3].R = Rb; + camMotions[3].t = -tb; + camMotions[3].n = -nb; +} + +} //namespace HomographyDecomposition + +int decomposeHomographyMat(InputArray _H, + InputArray _K, + OutputArrayOfArrays _rotations, + OutputArrayOfArrays _translations, + OutputArrayOfArrays _normals) +{ + using namespace std; + using namespace HomographyDecomposition; + + Mat H = _H.getMat().reshape(1, 3); + CV_Assert(H.cols == 3 && H.rows == 3); + + Mat K = _K.getMat().reshape(1, 3); + CV_Assert(K.cols == 3 && K.rows == 3); + + auto_ptr hdecomp(new HomographyDecompInria); + + vector motions; + hdecomp->decomposeHomography(H, K, motions); + + int nsols = static_cast(motions.size()); + int depth = CV_64F; //double precision matrices used in CameraMotion struct + + if (_rotations.needed()) { + _rotations.create(nsols, 1, depth); + for (int k = 0; k < nsols; ++k ) { + _rotations.getMatRef(k) = Mat(motions[k].R); + } + } + + if (_translations.needed()) { + _translations.create(nsols, 1, depth); + for (int k = 0; k < nsols; ++k ) { + _translations.getMatRef(k) = Mat(motions[k].t); + } + } + + if (_normals.needed()) { + _normals.create(nsols, 1, depth); + for (int k = 0; k < nsols; ++k ) { + _normals.getMatRef(k) = Mat(motions[k].n); + } + } + + return nsols; +} + +} //namespace cv diff --git a/modules/calib3d/test/test_homography_decomp.cpp b/modules/calib3d/test/test_homography_decomp.cpp new file mode 100644 index 000000000..dbe62c0c8 --- /dev/null +++ b/modules/calib3d/test/test_homography_decomp.cpp @@ -0,0 +1,138 @@ +/*M/////////////////////////////////////////////////////////////////////////////////////// + // + // This is a test file for the function decomposeHomography contributed to OpenCV + // by Samson Yilma. + // + // 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, Samson Yilma¸ (samson_yilma@yahoo.com), 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/calib3d.hpp" +#include + +using namespace cv; +using namespace std; + +class CV_HomographyDecompTest: public cvtest::BaseTest { + +public: + CV_HomographyDecompTest() + { + buildTestDataSet(); + } + +protected: + void run(int) + { + vector rotations; + vector translations; + vector normals; + + decomposeHomographyMat(_H, _K, rotations, translations, normals); + + //there should be at least 1 solution + ASSERT_GT(rotations.size(), 0); + ASSERT_GT(translations.size(), 0); + ASSERT_GT(normals.size(), 0); + + ASSERT_EQ(rotations.size(), normals.size()); + ASSERT_EQ(translations.size(), normals.size()); + + ASSERT_TRUE(containsValidMotion(rotations, translations, normals)); + + decomposeHomographyMat(_H, _K, rotations, noArray(), noArray()); + ASSERT_GT(rotations.size(), 0); + } + +private: + + void buildTestDataSet() + { + _K = Matx33d(640, 0.0, 320, + 0, 640, 240, + 0, 0, 1); + + _H = Matx33d(2.649157564634028, 4.583875997496426, 70.694447785121326, + -1.072756858861583, 3.533262150437228, 1513.656999614321649, + 0.001303887589576, 0.003042206876298, 1.000000000000000 + ); + + //expected solution for the given homography and intrinsic matrices + _R = Matx33d(0.43307983549125, 0.545749113549648, -0.717356090899523, + -0.85630229674426, 0.497582023798831, -0.138414255706431, + 0.281404038139784, 0.67421809131173, 0.682818960388909); + + _t = Vec3d(1.826751712278038, 1.264718492450820, 0.195080809998819); + _n = Vec3d(0.244875830334816, 0.480857890778889, 0.841909446789566); + } + + bool containsValidMotion(std::vector& rotations, + std::vector& translations, + std::vector& normals + ) + { + double max_error = 1.0e-3; + + vector::iterator riter = rotations.begin(); + vector::iterator titer = translations.begin(); + vector::iterator niter = normals.begin(); + + for (; + riter != rotations.end() && titer != translations.end() && niter != normals.end(); + ++riter, ++titer, ++niter) { + + double rdist = norm(*riter, _R, NORM_INF); + double tdist = norm(*titer, _t, NORM_INF); + double ndist = norm(*niter, _n, NORM_INF); + + if ( rdist < max_error + && tdist < max_error + && ndist < max_error ) + return true; + } + + return false; + } + + Matx33d _R, _K, _H; + Vec3d _t, _n; +}; + +TEST(Calib3d_DecomposeHomography, regression) { CV_HomographyDecompTest test; test.safe_run(); }