Merge pull request #3308 from edgarriba:master

modules/calib3d/doc/camera_calibration_and_3d_reconstruction.rst

@@ -588,6 +588,7 @@ Finds an object pose from 3D-2D point correspondences.
             *  **SOLVEPNP_P3P**  Method is based on the paper of X.S. Gao, X.-R. Hou, J. Tang, H.-F. Chang "Complete Solution Classification for the Perspective-Three-Point Problem". In this case the function requires exactly four object and image points.
             *  **SOLVEPNP_EPNP** Method has been introduced by F.Moreno-Noguer, V.Lepetit and P.Fua in the paper "EPnP: Efficient Perspective-n-Point Camera Pose Estimation".
             *  **SOLVEPNP_DLS**  Method is based on the paper of Joel A. Hesch and Stergios I. Roumeliotis. "A Direct Least-Squares (DLS) Method for PnP".
+            *  **SOLVEPNP_UPNP** Method is based on the paper of A.Penate-Sanchez, J.Andrade-Cetto, F.Moreno-Noguer. "Exhaustive Linearization for Robust Camera Pose and Focal Length Estimation". In this case the function also estimates the parameters :math:`f_x` and :math:`f_y` assuming that both have the same value. Then the ``cameraMatrix`` is updated with the estimated focal length.
 
 The function estimates the object pose given a set of object points, their corresponding image projections, as well as the camera matrix and the distortion coefficients.
 

modules/calib3d/include/opencv2/calib3d.hpp

@@ -59,7 +59,9 @@ enum { LMEDS  = 4, //!< least-median algorithm
 enum { SOLVEPNP_ITERATIVE = 0,
        SOLVEPNP_EPNP      = 1, // F.Moreno-Noguer, V.Lepetit and P.Fua "EPnP: Efficient Perspective-n-Point Camera Pose Estimation"
        SOLVEPNP_P3P       = 2, // X.S. Gao, X.-R. Hou, J. Tang, H.-F. Chang; "Complete Solution Classification for the Perspective-Three-Point Problem"
-       SOLVEPNP_DLS       = 3  // Joel A. Hesch and Stergios I. Roumeliotis. "A Direct Least-Squares (DLS) Method for PnP"
+       SOLVEPNP_DLS       = 3, // Joel A. Hesch and Stergios I. Roumeliotis. "A Direct Least-Squares (DLS) Method for PnP"
+       SOLVEPNP_UPNP      = 4  // A.Penate-Sanchez, J.Andrade-Cetto, F.Moreno-Noguer. "Exhaustive Linearization for Robust Camera Pose and Focal Length Estimation"
+
 };
 
 enum { CALIB_CB_ADAPTIVE_THRESH = 1,

modules/calib3d/perf/perf_pnp.cpp

@@ -10,7 +10,7 @@ using namespace perf;
 using std::tr1::make_tuple;
 using std::tr1::get;
 
-CV_ENUM(pnpAlgo, SOLVEPNP_ITERATIVE, SOLVEPNP_EPNP, SOLVEPNP_P3P, SOLVEPNP_DLS)
+CV_ENUM(pnpAlgo, SOLVEPNP_ITERATIVE, SOLVEPNP_EPNP, SOLVEPNP_P3P, SOLVEPNP_DLS, SOLVEPNP_UPNP)
 
 typedef std::tr1::tuple<int, pnpAlgo> PointsNum_Algo_t;
 typedef perf::TestBaseWithParam<PointsNum_Algo_t> PointsNum_Algo;
@@ -65,7 +65,7 @@ PERF_TEST_P(PointsNum_Algo, solvePnP,
 PERF_TEST_P(PointsNum_Algo, solvePnPSmallPoints,
             testing::Combine(
                 testing::Values(4), //TODO: find why results on 4 points are too unstable
-                testing::Values((int)SOLVEPNP_P3P, (int)SOLVEPNP_DLS)
+                testing::Values((int)SOLVEPNP_P3P, (int)SOLVEPNP_DLS, (int)SOLVEPNP_UPNP)
                 )
             )
 {
@@ -103,7 +103,7 @@ PERF_TEST_P(PointsNum_Algo, solvePnPSmallPoints,
         solvePnP(points3d, points2d, intrinsics, distortion, rvec, tvec, false, algo);
     }
 
-    SANITY_CHECK(rvec, 1e-4);
+    SANITY_CHECK(rvec, 1e-1);
     SANITY_CHECK(tvec, 1e-2);
 }
 

modules/calib3d/src/solvepnp.cpp

@@ -41,6 +41,7 @@
  //M*/
 
 #include "precomp.hpp"
+#include "upnp.h"
 #include "dls.h"
 #include "epnp.h"
 #include "p3p.h"
@@ -107,6 +108,16 @@ bool cv::solvePnP( InputArray _opoints, InputArray _ipoints,
             cv::Rodrigues(R, rvec);
         return result;
     }
+    else if (flags == SOLVEPNP_UPNP)
+    {
+        upnp PnP(cameraMatrix, opoints, ipoints);
+
+        cv::Mat R, rvec = _rvec.getMat(), tvec = _tvec.getMat();
+        double f = PnP.compute_pose(R, tvec);
+        cv::Rodrigues(R, rvec);
+        cameraMatrix.at<double>(0,0) = cameraMatrix.at<double>(1,1) = f;
+        return true;
+    }
     else
         CV_Error(CV_StsBadArg, "The flags argument must be one of SOLVEPNP_ITERATIVE, SOLVEPNP_P3P, SOLVEPNP_EPNP or SOLVEPNP_DLS");
     return false;
@@ -205,6 +216,7 @@ bool cv::solvePnPRansac(InputArray _opoints, InputArray _ipoints,
 
     int model_points = 4;                             // minimum of number of model points
     if( flags == cv::SOLVEPNP_ITERATIVE ) model_points = 6;
+    else if( flags == cv::SOLVEPNP_UPNP ) model_points = 6;
     else if( flags == cv::SOLVEPNP_EPNP ) model_points = 5;
 
     double param1 = reprojectionError;                // reprojection error

modules/calib3d/src/upnp.h

+//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, Intel Corporation, all rights reserved.
+// Copyright (C) 2013, 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.
+//
+//M*/
+
+/****************************************************************************************\
+* Exhaustive Linearization for Robust Camera Pose and Focal Length Estimation.
+* Contributed by Edgar Riba
+\****************************************************************************************/
+
+#ifndef OPENCV_CALIB3D_UPNP_H_
+#define OPENCV_CALIB3D_UPNP_H_
+
+#include "precomp.hpp"
+#include "opencv2/core/core_c.h"
+#include <iostream>
+
+class upnp
+{
+public:
+    upnp(const cv::Mat& cameraMatrix, const cv::Mat& opoints, const cv::Mat& ipoints);
+    ~upnp();
+
+    double compute_pose(cv::Mat& R, cv::Mat& t);
+private:
+    template <typename T>
+      void init_camera_parameters(const cv::Mat& cameraMatrix)
+      {
+        uc = cameraMatrix.at<T> (0, 2);
+        vc = cameraMatrix.at<T> (1, 2);
+        fu = 1;
+        fv = 1;
+      }
+      template <typename OpointType, typename IpointType>
+      void init_points(const cv::Mat& opoints, const cv::Mat& ipoints)
+      {
+          for(int i = 0; i < number_of_correspondences; i++)
+          {
+            pws[3 * i    ] = opoints.at<OpointType>(0,i).x;
+            pws[3 * i + 1] = opoints.at<OpointType>(0,i).y;
+            pws[3 * i + 2] = opoints.at<OpointType>(0,i).z;
+
+            us[2 * i    ] = ipoints.at<IpointType>(0,i).x;
+            us[2 * i + 1] = ipoints.at<IpointType>(0,i).y;
+          }
+      }
+
+      double reprojection_error(const double R[3][3], const double t[3]);
+      void choose_control_points();
+      void compute_alphas();
+      void fill_M(cv::Mat * M, const int row, const double * alphas, const double u, const double v);
+      void compute_ccs(const double * betas, const double * ut);
+      void compute_pcs(void);
+
+      void solve_for_sign(void);
+
+      void find_betas_and_focal_approx_1(cv::Mat * Ut, cv::Mat * Rho, double * betas, double * efs);
+      void find_betas_and_focal_approx_2(cv::Mat * Ut, cv::Mat * Rho, double * betas, double * efs);
+      void qr_solve(cv::Mat * A, cv::Mat * b, cv::Mat * X);
+
+      cv::Mat compute_constraint_distance_2param_6eq_2unk_f_unk(const cv::Mat& M1);
+      cv::Mat compute_constraint_distance_3param_6eq_6unk_f_unk(const cv::Mat& M1, const cv::Mat& M2);
+      void generate_all_possible_solutions_for_f_unk(const double betas[5], double solutions[18][3]);
+
+      double sign(const double v);
+      double dot(const double * v1, const double * v2);
+      double dotXY(const double * v1, const double * v2);
+      double dotZ(const double * v1, const double * v2);
+      double dist2(const double * p1, const double * p2);
+
+      void compute_rho(double * rho);
+      void compute_L_6x12(const double * ut, double * l_6x12);
+
+      void gauss_newton(const cv::Mat * L_6x12, const cv::Mat * Rho, double current_betas[4], double * efs);
+      void compute_A_and_b_gauss_newton(const double * l_6x12, const double * rho,
+                          const double cb[4], cv::Mat * A, cv::Mat * b, double const f);
+
+      double compute_R_and_t(const double * ut, const double * betas,
+                   double R[3][3], double t[3]);
+
+      void estimate_R_and_t(double R[3][3], double t[3]);
+
+      void copy_R_and_t(const double R_dst[3][3], const double t_dst[3],
+                  double R_src[3][3], double t_src[3]);
+
+
+      double uc, vc, fu, fv;
+
+      std::vector<double> pws, us, alphas, pcs;
+      int number_of_correspondences;
+
+      double cws[4][3], ccs[4][3];
+      int max_nr;
+      double * A1, * A2;
+};
+
+#endif // OPENCV_CALIB3D_UPNP_H_

modules/calib3d/test/test_solvepnp_ransac.cpp

@@ -58,6 +58,7 @@ public:
         eps[SOLVEPNP_EPNP] = 1.0e-2;
         eps[SOLVEPNP_P3P] = 1.0e-2;
         eps[SOLVEPNP_DLS] = 1.0e-2;
+        eps[SOLVEPNP_UPNP] = 1.0e-2;
         totalTestsCount = 10;
     }
     ~CV_solvePnPRansac_Test() {}
@@ -118,6 +119,7 @@ protected:
         Mat trueRvec, trueTvec;
         Mat intrinsics, distCoeffs;
         generateCameraMatrix(intrinsics, rng);
+        if (method == 4) intrinsics.at<double>(1,1) = intrinsics.at<double>(0,0);
         if (mode == 0)
             distCoeffs = Mat::zeros(4, 1, CV_64FC1);
         else
@@ -159,7 +161,7 @@ protected:
         points.resize(pointsCount);
         generate3DPointCloud(points);
 
-        const int methodsCount = 4;
+        const int methodsCount = 5;
         RNG rng = ts->get_rng();
 
 
@@ -184,7 +186,7 @@ protected:
             }
         }
     }
-    double eps[4];
+    double eps[5];
     int totalTestsCount;
 };
 
@@ -197,6 +199,7 @@ public:
         eps[SOLVEPNP_EPNP] = 1.0e-6;
         eps[SOLVEPNP_P3P] = 1.0e-4;
         eps[SOLVEPNP_DLS] = 1.0e-4;
+        eps[SOLVEPNP_UPNP] = 1.0e-4;
         totalTestsCount = 1000;
     }
 
@@ -208,6 +211,7 @@ protected:
         Mat trueRvec, trueTvec;
         Mat intrinsics, distCoeffs;
         generateCameraMatrix(intrinsics, rng);
+        if (method == 4) intrinsics.at<double>(1,1) = intrinsics.at<double>(0,0);
         if (mode == 0)
             distCoeffs = Mat::zeros(4, 1, CV_64FC1);
         else