Merge remote-tracking branch 'upstream/3.4' into merge-3.4

diff --cc modules/calib3d/include/opencv2/calib3d.hpp
index 264c976,16ab334..5f6d3c0
--- 1/modules/calib3d/include/opencv2/calib3d.hpp
--- 2/modules/calib3d/include/opencv2/calib3d.hpp
+++ b/modules/calib3d/include/opencv2/calib3d.hpp
@@@ -323,68 -320,6 +323,71 @@@ An example program about pose estimatio
  Check @ref tutorial_homography "the corresponding tutorial" for more details
  */
  
 +/** Levenberg-Marquardt solver. Starting with the specified vector of parameters it
 +    optimizes the target vector criteria "err"
 +    (finds local minima of each target vector component absolute value).
 +
 +    When needed, it calls user-provided callback.
 +*/
 +class CV_EXPORTS LMSolver : public Algorithm
 +{
 +public:
 +    class CV_EXPORTS Callback
 +    {
 +    public:
 +        virtual ~Callback() {}
 +        /**
 +         computes error and Jacobian for the specified vector of parameters
 +
 +         @param param the current vector of parameters
 +         @param err output vector of errors: err_i = actual_f_i - ideal_f_i
 +         @param J output Jacobian: J_ij = d(err_i)/d(param_j)
 +
 +         when J=noArray(), it means that it does not need to be computed.
 +         Dimensionality of error vector and param vector can be different.
 +         The callback should explicitly allocate (with "create" method) each output array
 +         (unless it's noArray()).
 +        */
 +        virtual bool compute(InputArray param, OutputArray err, OutputArray J) const = 0;
 +    };
 +
 +    /**
 +       Runs Levenberg-Marquardt algorithm using the passed vector of parameters as the start point.
 +       The final vector of parameters (whether the algorithm converged or not) is stored at the same
 +       vector. The method returns the number of iterations used. If it's equal to the previously specified
 +       maxIters, there is a big chance the algorithm did not converge.
 +
 +       @param param initial/final vector of parameters.
 +
 +       Note that the dimensionality of parameter space is defined by the size of param vector,
 +       and the dimensionality of optimized criteria is defined by the size of err vector
 +       computed by the callback.
 +    */
 +    virtual int run(InputOutputArray param) const = 0;
 +
 +    /**
 +       Sets the maximum number of iterations
 +       @param maxIters the number of iterations
 +    */
 +    virtual void setMaxIters(int maxIters) = 0;
 +    /**
 +       Retrieves the current maximum number of iterations
 +    */
 +    virtual int getMaxIters() const = 0;
 +
 +    /**
 +       Creates Levenberg-Marquard solver
 +
 +       @param cb callback
 +       @param maxIters maximum number of iterations that can be further
 +         modified using setMaxIters() method.
 +    */
 +    static Ptr<LMSolver> create(const Ptr<LMSolver::Callback>& cb, int maxIters);
++    static Ptr<LMSolver> create(const Ptr<LMSolver::Callback>& cb, int maxIters, double eps);
 +};
 +
++
++
  /** @brief Finds a perspective transformation between two planes.
  
  @param srcPoints Coordinates of the points in the original plane, a matrix of the type CV_32FC2

diff --cc modules/calib3d/src/levmarq.cpp
index 4e59f04,a0172b2..0d339cc
--- 1/modules/calib3d/src/levmarq.cpp
--- 2/modules/calib3d/src/levmarq.cpp
+++ b/modules/calib3d/src/levmarq.cpp
@@@ -215,4 -214,9 +215,9 @@@ Ptr<LMSolver> LMSolver::create(const Pt
      return makePtr<LMSolverImpl>(cb, maxIters);
  }
  
 -Ptr<LMSolver> createLMSolver(const Ptr<LMSolver::Callback>& cb, int maxIters, double eps)
++Ptr<LMSolver> LMSolver::create(const Ptr<LMSolver::Callback>& cb, int maxIters, double eps)
+ {
+     return makePtr<LMSolverImpl>(cb, maxIters, eps);
+ }
+ 
  }

diff --cc modules/calib3d/src/solvepnp.cpp
index 0f76e8c,82a1604..58c16f4
--- 1/modules/calib3d/src/solvepnp.cpp
--- 2/modules/calib3d/src/solvepnp.cpp
+++ b/modules/calib3d/src/solvepnp.cpp
@@@ -456,4 -456,271 +456,271 @@@ int solveP3P( InputArray _opoints, Inpu
      return solutions;
  }
  
 -        createLMSolver(makePtr<SolvePnPRefineLMCallback>(opoints, ipoints, cameraMatrix, distCoeffs), _criteria.maxCount, _criteria.epsilon)->run(params);
+ class SolvePnPRefineLMCallback CV_FINAL : public LMSolver::Callback
+ {
+ public:
+     SolvePnPRefineLMCallback(InputArray _opoints, InputArray _ipoints, InputArray _cameraMatrix, InputArray _distCoeffs)
+     {
+         objectPoints = _opoints.getMat();
+         imagePoints = _ipoints.getMat();
+         npoints = std::max(objectPoints.checkVector(3, CV_32F), objectPoints.checkVector(3, CV_64F));
+         imagePoints0 = imagePoints.reshape(1, npoints*2);
+         cameraMatrix = _cameraMatrix.getMat();
+         distCoeffs = _distCoeffs.getMat();
+     }
+ 
+     bool compute(InputArray _param, OutputArray _err, OutputArray _Jac) const CV_OVERRIDE
+     {
+          Mat param = _param.getMat();
+          _err.create(npoints*2, 1, CV_64FC1);
+ 
+          if(_Jac.needed())
+          {
+              _Jac.create(npoints*2, param.rows, CV_64FC1);
+          }
+ 
+          Mat rvec = param(Rect(0, 0, 1, 3)), tvec = param(Rect(0, 3, 1, 3));
+ 
+          Mat J, projectedPts;
+          projectPoints(objectPoints, rvec, tvec, cameraMatrix, distCoeffs, projectedPts, _Jac.needed() ? J : noArray());
+ 
+          if (_Jac.needed())
+          {
+              Mat Jac = _Jac.getMat();
+              for (int i = 0; i < Jac.rows; i++)
+              {
+                  for (int j = 0; j < Jac.cols; j++)
+                  {
+                      Jac.at<double>(i,j) = J.at<double>(i,j);
+                  }
+              }
+          }
+ 
+          Mat err = _err.getMat();
+          projectedPts = projectedPts.reshape(1, npoints*2);
+          err = projectedPts - imagePoints0;
+ 
+         return true;
+     }
+ 
+     Mat objectPoints, imagePoints, imagePoints0;
+     Mat cameraMatrix, distCoeffs;
+     int npoints;
+ };
+ 
+ /**
+  * @brief Compute the Interaction matrix and the residuals for the current pose.
+  * @param objectPoints 3D object points.
+  * @param R Current estimated rotation matrix.
+  * @param tvec Current estimated translation vector.
+  * @param L Interaction matrix for a vector of point features.
+  * @param s Residuals.
+  */
+ static void computeInteractionMatrixAndResiduals(const Mat& objectPoints, const Mat& R, const Mat& tvec,
+                                                  Mat& L, Mat& s)
+ {
+     Mat objectPointsInCam;
+ 
+     int npoints = objectPoints.rows;
+     for (int i = 0; i < npoints; i++)
+     {
+         Mat curPt = objectPoints.row(i);
+         objectPointsInCam = R * curPt.t() + tvec;
+ 
+         double Zi = objectPointsInCam.at<double>(2,0);
+         double xi = objectPointsInCam.at<double>(0,0) / Zi;
+         double yi = objectPointsInCam.at<double>(1,0) / Zi;
+ 
+         s.at<double>(2*i,0) = xi;
+         s.at<double>(2*i+1,0) = yi;
+ 
+         L.at<double>(2*i,0) = -1 / Zi;
+         L.at<double>(2*i,1) = 0;
+         L.at<double>(2*i,2) = xi / Zi;
+         L.at<double>(2*i,3) = xi*yi;
+         L.at<double>(2*i,4) = -(1 + xi*xi);
+         L.at<double>(2*i,5) = yi;
+ 
+         L.at<double>(2*i+1,0) = 0;
+         L.at<double>(2*i+1,1) = -1 / Zi;
+         L.at<double>(2*i+1,2) = yi / Zi;
+         L.at<double>(2*i+1,3) = 1 + yi*yi;
+         L.at<double>(2*i+1,4) = -xi*yi;
+         L.at<double>(2*i+1,5) = -xi;
+     }
+ }
+ 
+ /**
+  * @brief The exponential map from se(3) to SE(3).
+  * @param twist A twist (v, w) represents the velocity of a rigid body as an angular velocity
+  * around an axis and a linear velocity along this axis.
+  * @param R1 Resultant rotation matrix from the twist.
+  * @param t1 Resultant translation vector from the twist.
+  */
+ static void exponentialMapToSE3Inv(const Mat& twist, Mat& R1, Mat& t1)
+ {
+     //see Exponential Map in http://ethaneade.com/lie.pdf
+     /*
+     \begin{align*}
+     \boldsymbol{\delta} &= \left( \mathbf{u}, \boldsymbol{\omega} \right ) \in se(3) \\
+     \mathbf{u}, \boldsymbol{\omega} &\in \mathbb{R}^3 \\
+     \theta &= \sqrt{ \boldsymbol{\omega}^T \boldsymbol{\omega} } \\
+     A &= \frac{\sin \theta}{\theta} \\
+     B &= \frac{1 - \cos \theta}{\theta^2} \\
+     C &= \frac{1-A}{\theta^2} \\
+     \mathbf{R} &= \mathbf{I} + A \boldsymbol{\omega}_{\times} + B \boldsymbol{\omega}_{\times}^2 \\
+     \mathbf{V} &= \mathbf{I} + B \boldsymbol{\omega}_{\times} + C \boldsymbol{\omega}_{\times}^2 \\
+     \exp \begin{pmatrix}
+     \mathbf{u} \\
+     \boldsymbol{\omega}
+     \end{pmatrix} &=
+     \left(
+     \begin{array}{c|c}
+     \mathbf{R} & \mathbf{V} \mathbf{u} \\ \hline
+     \mathbf{0} & 1
+     \end{array}
+     \right )
+     \end{align*}
+     */
+     double vx = twist.at<double>(0,0);
+     double vy = twist.at<double>(1,0);
+     double vz = twist.at<double>(2,0);
+     double wx = twist.at<double>(3,0);
+     double wy = twist.at<double>(4,0);
+     double wz = twist.at<double>(5,0);
+ 
+     Matx31d rvec(wx, wy, wz);
+     Mat R;
+     Rodrigues(rvec, R);
+ 
+     double theta = sqrt(wx*wx + wy*wy + wz*wz);
+     double sinc = std::fabs(theta) < 1e-8 ? 1 : sin(theta) / theta;
+     double mcosc = (std::fabs(theta) < 1e-8) ? 0.5 : (1-cos(theta)) / (theta*theta);
+     double msinc = (std::abs(theta) < 1e-8) ? (1/6.0) : (1-sinc) / (theta*theta);
+ 
+     Matx31d dt;
+     dt(0) = vx*(sinc + wx*wx*msinc) + vy*(wx*wy*msinc - wz*mcosc) + vz*(wx*wz*msinc + wy*mcosc);
+     dt(1) = vx*(wx*wy*msinc + wz*mcosc) + vy*(sinc + wy*wy*msinc) + vz*(wy*wz*msinc - wx*mcosc);
+     dt(2) = vx*(wx*wz*msinc - wy*mcosc) + vy*(wy*wz*msinc + wx*mcosc) + vz*(sinc + wz*wz*msinc);
+ 
+     R1 = R.t();
+     t1 = -R1 * dt;
+ }
+ 
+ enum SolvePnPRefineMethod {
+     SOLVEPNP_REFINE_LM   = 0,
+     SOLVEPNP_REFINE_VVS  = 1
+ };
+ 
+ static void solvePnPRefine(InputArray _objectPoints, InputArray _imagePoints,
+                            InputArray _cameraMatrix, InputArray _distCoeffs,
+                            InputOutputArray _rvec, InputOutputArray _tvec,
+                            SolvePnPRefineMethod _flags,
+                            TermCriteria _criteria=TermCriteria(TermCriteria::EPS+TermCriteria::COUNT, 20, FLT_EPSILON),
+                            double _vvslambda=1)
+ {
+     CV_INSTRUMENT_REGION();
+ 
+     Mat opoints_ = _objectPoints.getMat(), ipoints_ = _imagePoints.getMat();
+     Mat opoints, ipoints;
+     opoints_.convertTo(opoints, CV_64F);
+     ipoints_.convertTo(ipoints, CV_64F);
+     int npoints = opoints.checkVector(3, CV_64F);
+     CV_Assert( npoints >= 3 && npoints == ipoints.checkVector(2, CV_64F) );
+     CV_Assert( !_rvec.empty() && !_tvec.empty() );
+ 
+     int rtype = _rvec.type(), ttype = _tvec.type();
+     Size rsize = _rvec.size(), tsize = _tvec.size();
+     CV_Assert( (rtype == CV_32FC1 || rtype == CV_64FC1) &&
+                (ttype == CV_32FC1 || ttype == CV_64FC1) );
+     CV_Assert( (rsize == Size(1, 3) || rsize == Size(3, 1)) &&
+                (tsize == Size(1, 3) || tsize == Size(3, 1)) );
+ 
+     Mat cameraMatrix0 = _cameraMatrix.getMat();
+     Mat distCoeffs0 = _distCoeffs.getMat();
+     Mat cameraMatrix = Mat_<double>(cameraMatrix0);
+     Mat distCoeffs = Mat_<double>(distCoeffs0);
+ 
+     if (_flags == SOLVEPNP_REFINE_LM)
+     {
+         Mat rvec0 = _rvec.getMat(), tvec0 = _tvec.getMat();
+         Mat rvec, tvec;
+         rvec0.convertTo(rvec, CV_64F);
+         tvec0.convertTo(tvec, CV_64F);
+ 
+         Mat params(6, 1, CV_64FC1);
+         for (int i = 0; i < 3; i++)
+         {
+             params.at<double>(i,0) = rvec.at<double>(i,0);
+             params.at<double>(i+3,0) = tvec.at<double>(i,0);
+         }
+ 
++        LMSolver::create(makePtr<SolvePnPRefineLMCallback>(opoints, ipoints, cameraMatrix, distCoeffs), _criteria.maxCount, _criteria.epsilon)->run(params);
+ 
+         params.rowRange(0, 3).convertTo(rvec0, rvec0.depth());
+         params.rowRange(3, 6).convertTo(tvec0, tvec0.depth());
+     }
+     else if (_flags == SOLVEPNP_REFINE_VVS)
+     {
+         Mat rvec0 = _rvec.getMat(), tvec0 = _tvec.getMat();
+         Mat rvec, tvec;
+         rvec0.convertTo(rvec, CV_64F);
+         tvec0.convertTo(tvec, CV_64F);
+ 
+         vector<Point2d> ipoints_normalized;
+         undistortPoints(ipoints, ipoints_normalized, cameraMatrix, distCoeffs);
+         Mat sd = Mat(ipoints_normalized).reshape(1, npoints*2);
+         Mat objectPoints0 = opoints.reshape(1, npoints);
+         Mat imagePoints0 = ipoints.reshape(1, npoints*2);
+         Mat L(npoints*2, 6, CV_64FC1), s(npoints*2, 1, CV_64FC1);
+ 
+         double residuals_1 = std::numeric_limits<double>::max(), residuals = 0;
+         Mat err;
+         Mat R;
+         Rodrigues(rvec, R);
+         for (int iter = 0; iter < _criteria.maxCount; iter++)
+         {
+             computeInteractionMatrixAndResiduals(objectPoints0, R, tvec, L, s);
+             err = s - sd;
+ 
+             Mat Lp = L.inv(cv::DECOMP_SVD);
+             Mat dq = -_vvslambda * Lp * err;
+ 
+             Mat R1, t1;
+             exponentialMapToSE3Inv(dq, R1, t1);
+             R = R1 * R;
+             tvec = R1 * tvec + t1;
+ 
+             residuals_1 = residuals;
+             Mat res = err.t()*err;
+             residuals = res.at<double>(0,0);
+ 
+             if (std::fabs(residuals - residuals_1) < _criteria.epsilon)
+                 break;
+         }
+ 
+         Rodrigues(R, rvec);
+         rvec.convertTo(rvec0, rvec0.depth());
+         tvec.convertTo(tvec0, tvec0.depth());
+     }
+ }
+ 
+ void solvePnPRefineLM(InputArray _objectPoints, InputArray _imagePoints,
+                       InputArray _cameraMatrix, InputArray _distCoeffs,
+                       InputOutputArray _rvec, InputOutputArray _tvec,
+                       TermCriteria _criteria)
+ {
+     CV_INSTRUMENT_REGION();
+     solvePnPRefine(_objectPoints, _imagePoints, _cameraMatrix, _distCoeffs, _rvec, _tvec, SOLVEPNP_REFINE_LM, _criteria);
+ }
+ 
+ void solvePnPRefineVVS(InputArray _objectPoints, InputArray _imagePoints,
+                        InputArray _cameraMatrix, InputArray _distCoeffs,
+                        InputOutputArray _rvec, InputOutputArray _tvec,
+                        TermCriteria _criteria, double _VVSlambda)
+ {
+     CV_INSTRUMENT_REGION();
+     solvePnPRefine(_objectPoints, _imagePoints, _cameraMatrix, _distCoeffs, _rvec, _tvec, SOLVEPNP_REFINE_VVS, _criteria, _VVSlambda);
+ }
+ 
  }

diff --cc modules/core/include/opencv2/core/cvdef.h
Simple merge

diff --cc modules/core/src/system.cpp
Simple merge

diff --cc modules/dnn/src/layers/convolution_layer.cpp
Simple merge

diff --cc modules/dnn/src/layers/pooling_layer.cpp
Simple merge

diff --cc modules/dnn/src/op_inf_engine.cpp
Simple merge

diff --cc modules/dnn/src/tensorflow/tf_graph_simplifier.cpp
Simple merge

diff --cc modules/dnn/src/tensorflow/tf_graph_simplifier.hpp
Simple merge

diff --cc modules/dnn/src/tensorflow/tf_importer.cpp
Simple merge

diff --cc modules/dnn/test/test_misc.cpp
Simple merge