Download modules/calib3d/src/homography_decomp.cpp from tgollogly/test: direct link, hf CLI and curl.
- Browser
- Download file 17.9 kB
-
https://huggingface.co/tgollogly/test/resolve/main/modules/calib3d/src/homography_decomp.cpp
- Command line
-
hf download hf://tgollogly/test/modules/calib3d/src/homography_decomp.cpp
-
curl -L -o homography_decomp.cpp https://huggingface.co/tgollogly/test/resolve/main/modules/calib3d/src/homography_decomp.cpp
17.9 kB
| /*M/////////////////////////////////////////////////////////////////////////////////////// | |
| // | |
| // This is a homography decomposition implementation contributed to OpenCV | |
| // by Samson Yilma. It implements the homography decomposition algorithm | |
| // described 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. | |
| // Copyright (C) 2018, Intel Corporation, 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*/ | |
| 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<CameraMotion>& camMotions); | |
| bool isRotationValid(const cv::Matx33d& R, const double epsilon=0.01); | |
| protected: | |
| bool passesSameSideOfPlaneConstraint(CameraMotion& motion); | |
| virtual void decompose(std::vector<CameraMotion>& camMotions) = 0; | |
| const cv::Matx33d& getHnorm() const { | |
| return _Hnorm; | |
| } | |
| private: | |
| /** | |
| * Normalize the homograhpy \f$H\f$. | |
| * | |
| * @param H Homography matrix. | |
| * @param K Intrinsic parameter matrix. | |
| * @return It returns | |
| * \f[ | |
| * K^{-1} * H * K | |
| * \f] | |
| */ | |
| cv::Matx33d normalize(const cv::Matx33d& H, const cv::Matx33d& K); | |
| void removeScale(); | |
| cv::Matx33d _Hnorm; | |
| }; | |
| class HomographyDecompZhang CV_FINAL : public HomographyDecomp { | |
| public: | |
| HomographyDecompZhang():HomographyDecomp() {} | |
| virtual ~HomographyDecompZhang() {} | |
| private: | |
| virtual void decompose(std::vector<CameraMotion>& camMotions) CV_OVERRIDE; | |
| bool findMotionFrom_tstar_n(const cv::Vec3d& tstar, const cv::Vec3d& n, CameraMotion& motion); | |
| }; | |
| class HomographyDecompInria CV_FINAL : public HomographyDecomp { | |
| public: | |
| HomographyDecompInria():HomographyDecomp() {} | |
| virtual ~HomographyDecompInria() {} | |
| private: | |
| virtual void decompose(std::vector<CameraMotion>& camMotions) CV_OVERRIDE; | |
| 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<double>(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<double, 1, 1> 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<CameraMotion>& 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(); | |
| if (cv::determinant(motion.R) < 0) | |
| { | |
| motion.R *= -1; | |
| } | |
| motion.t = motion.R * tstar; | |
| motion.n = n; | |
| return passesSameSideOfPlaneConstraint(motion); | |
| } | |
| void HomographyDecompZhang::decompose(std::vector<CameraMotion>& camMotions) | |
| { | |
| Mat W, U, Vt; | |
| SVD::compute(getHnorm(), W, U, Vt); | |
| CV_Assert(W.total() > 2 && Vt.total() > 7); | |
| double lambda1=W.at<double>(0); | |
| double lambda3=W.at<double>(2); | |
| double lambda1m3 = (lambda1-lambda3); | |
| double lambda1m3_2 = lambda1m3*lambda1m3; | |
| double lambda1t3 = lambda1*lambda3; | |
| double t1 = 1.0/(2.0*lambda1t3); | |
| double t2 = sqrt(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 = sqrt(e1_2*lambda1m3_2 + 2*e1*(lambda1t3-1) + 1.0); | |
| double nv3p = sqrt(e3_2*lambda1m3_2 + 2*e3*(lambda1t3-1) + 1.0); | |
| double v1p[3], v3p[3]; | |
| v1p[0]=Vt.at<double>(0)*nv1p, v1p[1]=Vt.at<double>(1)*nv1p, v1p[2]=Vt.at<double>(2)*nv1p; | |
| v3p[0]=Vt.at<double>(6)*nv3p, v3p[1]=Vt.at<double>(7)*nv3p, v3p[2]=Vt.at<double>(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() ); | |
| if (cv::determinant(R) < 0) | |
| { | |
| R *= -1; | |
| } | |
| } | |
| void HomographyDecompInria::decompose(std::vector<CameraMotion>& 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 * sqrt(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 | |
| // function decomposes image-to-image homography to rotation and translation matrices | |
| 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); | |
| cv::Ptr<HomographyDecomp> hdecomp(new HomographyDecompInria); | |
| vector<CameraMotion> motions; | |
| hdecomp->decomposeHomography(H, K, motions); | |
| int nsols = static_cast<int>(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; | |
| } | |
| void filterHomographyDecompByVisibleRefpoints(InputArrayOfArrays _rotations, | |
| InputArrayOfArrays _normals, | |
| InputArray _beforeRectifiedPoints, | |
| InputArray _afterRectifiedPoints, | |
| OutputArray _possibleSolutions, | |
| InputArray _pointsMask) | |
| { | |
| CV_Assert(_beforeRectifiedPoints.type() == CV_32FC2 && _afterRectifiedPoints.type() == CV_32FC2); | |
| CV_Assert(_pointsMask.empty() || _pointsMask.type() == CV_8U); | |
| Mat beforeRectifiedPoints = _beforeRectifiedPoints.getMat(); | |
| Mat afterRectifiedPoints = _afterRectifiedPoints.getMat(); | |
| Mat pointsMask = _pointsMask.getMat(); | |
| int nsolutions = (int)_rotations.total(); | |
| int npoints = (int)beforeRectifiedPoints.total(); | |
| CV_Assert(pointsMask.empty() || pointsMask.checkVector(1, CV_8U) == npoints); | |
| const uchar* pointsMaskPtr = pointsMask.data; | |
| std::vector<uchar> solutionMask(nsolutions, (uchar)1); | |
| std::vector<Mat> normals(nsolutions); | |
| std::vector<Mat> rotnorm(nsolutions); | |
| Mat R; | |
| for( int i = 0; i < nsolutions; i++ ) | |
| { | |
| _normals.getMat(i).convertTo(normals[i], CV_64F); | |
| CV_Assert(normals[i].total() == 3); | |
| _rotations.getMat(i).convertTo(R, CV_64F); | |
| rotnorm[i] = R*normals[i]; | |
| CV_Assert(rotnorm[i].total() == 3); | |
| } | |
| for( int j = 0; j < npoints; j++ ) | |
| { | |
| if( !pointsMaskPtr || pointsMaskPtr[j] ) | |
| { | |
| Point2f prevPoint = beforeRectifiedPoints.at<Point2f>(j); | |
| Point2f currPoint = afterRectifiedPoints.at<Point2f>(j); | |
| for( int i = 0; i < nsolutions; i++ ) | |
| { | |
| if( !solutionMask[i] ) | |
| continue; | |
| const double* normal_i = normals[i].ptr<double>(); | |
| const double* rotnorm_i = rotnorm[i].ptr<double>(); | |
| double prevNormDot = normal_i[0]*prevPoint.x + normal_i[1]*prevPoint.y + normal_i[2]; | |
| double currNormDot = rotnorm_i[0]*currPoint.x + rotnorm_i[1]*currPoint.y + rotnorm_i[2]; | |
| if (prevNormDot <= 0 || currNormDot <= 0) | |
| solutionMask[i] = (uchar)0; | |
| } | |
| } | |
| } | |
| std::vector<int> possibleSolutions; | |
| for( int i = 0; i < nsolutions; i++ ) | |
| if( solutionMask[i] ) | |
| possibleSolutions.push_back(i); | |
| Mat(possibleSolutions).copyTo(_possibleSolutions); | |
| } | |
| } //namespace cv | |