--- /dev/null
+/*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<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:
+ 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<CameraMotion>& 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<CameraMotion>& 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<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();
+ 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);
+ 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 = 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<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() );
+}
+
+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 * 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<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;
+}
+
+} //namespace cv
--- /dev/null
+/*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 <vector>
+
+using namespace cv;
+using namespace std;
+
+class CV_HomographyDecompTest: public cvtest::BaseTest {
+
+public:
+ CV_HomographyDecompTest()
+ {
+ buildTestDataSet();
+ }
+
+protected:
+ void run(int)
+ {
+ vector<Mat> rotations;
+ vector<Mat> translations;
+ vector<Mat> 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<Mat>& rotations,
+ std::vector<Mat>& translations,
+ std::vector<Mat>& normals
+ )
+ {
+ double max_error = 1.0e-3;
+
+ vector<Mat>::iterator riter = rotations.begin();
+ vector<Mat>::iterator titer = translations.begin();
+ vector<Mat>::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(); }
projects / platform / upstream / opencv.git / commitdiff