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

Author: alalek

doc/mymath.js

@@ -9,6 +9,9 @@ MathJax.Hub.Config(
           forkfour: ["\\left\\{ \\begin{array}{l l} #1 & \\mbox{#2}\\\\ #3 & \\mbox{#4}\\\\ #5 & \\mbox{#6}\\\\ #7 & \\mbox{#8}\\\\ \\end{array} \\right.", 8],
           vecthree: ["\\begin{bmatrix} #1\\\\ #2\\\\ #3 \\end{bmatrix}", 3],
           vecthreethree: ["\\begin{bmatrix} #1 & #2 & #3\\\\ #4 & #5 & #6\\\\ #7 & #8 & #9 \\end{bmatrix}", 9],
+          cameramatrix: ["#1 = \\begin{bmatrix} f_x & 0 & c_x\\\\ 0 & f_y & c_y\\\\ 0 & 0 & 1 \\end{bmatrix}", 1],
+          distcoeffs: ["(k_1, k_2, p_1, p_2[, k_3[, k_4, k_5, k_6 [, s_1, s_2, s_3, s_4[, \\tau_x, \\tau_y]]]]) \\text{ of 4, 5, 8, 12 or 14 elements}"],
+          distcoeffsfisheye: ["(k_1, k_2, k_3, k_4)"],
           hdotsfor: ["\\dots", 1],
           mathbbm: ["\\mathbb{#1}", 1],
           bordermatrix: ["\\matrix{#1}", 1]

doc/mymath.sty

@@ -51,3 +51,20 @@
  #7 & #8 & #9
 \end{bmatrix}
 }
+
+\newcommand{\cameramatrix}[1]{
+#1 =
+\begin{bmatrix}
+ f_x & 0 & c_x\\
+ 0 & f_y & c_y\\
+ 0 & 0 & 1
+\end{bmatrix}
+}
+
+\newcommand{\distcoeffs}[]{
+(k_1, k_2, p_1, p_2[, k_3[, k_4, k_5, k_6 [, s_1, s_2, s_3, s_4[, \tau_x, \tau_y]]]]) \text{ of 4, 5, 8, 12 or 14 elements}
+}
+
+\newcommand{\distcoeffsfisheye}[]{
+(k_1, k_2, k_3, k_4)
+}

doc/pattern_tools/gen_pattern.py

@@ -36,18 +36,27 @@ def __init__(self, cols, rows, output, units, square_size, radius_rate, page_wid
     def make_circles_pattern(self):
         spacing = self.square_size
         r = spacing / self.radius_rate
-        for x in range(1, self.cols + 1):
-            for y in range(1, self.rows + 1):
-                dot = SVG("circle", cx=x * spacing, cy=y * spacing, r=r, fill="black", stroke="none")
+        pattern_width = ((self.cols - 1.0) * spacing) + (2.0 * r)
+        pattern_height = ((self.rows - 1.0) * spacing) + (2.0 * r)
+        x_spacing = (self.width - pattern_width) / 2.0
+        y_spacing = (self.height - pattern_height) / 2.0
+        for x in range(0, self.cols):
+            for y in range(0, self.rows):
+                dot = SVG("circle", cx=(x * spacing) + x_spacing + r,
+                          cy=(y * spacing) + y_spacing + r, r=r, fill="black", stroke="none")
                 self.g.append(dot)
 
     def make_acircles_pattern(self):
         spacing = self.square_size
         r = spacing / self.radius_rate
-        for i in range(0, self.rows):
-            for j in range(0, self.cols):
-                dot = SVG("circle", cx=((j * 2 + i % 2) * spacing) + spacing, cy=self.height - (i * spacing + spacing),
-                          r=r, fill="black", stroke="none")
+        pattern_width = ((self.cols-1.0) * 2 * spacing) + spacing + (2.0 * r)
+        pattern_height = ((self.rows-1.0) * spacing) + (2.0 * r)
+        x_spacing = (self.width - pattern_width) / 2.0
+        y_spacing = (self.height - pattern_height) / 2.0
+        for x in range(0, self.cols):
+            for y in range(0, self.rows):
+                dot = SVG("circle", cx=(2 * x * spacing) + (y % 2)*spacing + x_spacing + r,
+                          cy=(y * spacing) + y_spacing + r, r=r, fill="black", stroke="none")
                 self.g.append(dot)
 
     def make_checkerboard_pattern(self):
@@ -84,9 +93,9 @@ def main():
     parser.add_argument("-R", "--radius_rate", help="circles_radius = square_size/radius_rate", default="5.0",
                         action="store", dest="radius_rate", type=float)
     parser.add_argument("-w", "--page_width", help="page width in units", default="216", action="store",
-                        dest="page_width", type=int)
+                        dest="page_width", type=float)
     parser.add_argument("-h", "--page_height", help="page height in units", default="279", action="store",
-                        dest="page_width", type=int)
+                        dest="page_width", type=float)
     parser.add_argument("-a", "--page_size", help="page size, supersedes -h -w arguments", default="A4", action="store",
                         dest="page_size", choices=["A0", "A1", "A2", "A3", "A4", "A5"])
     args = parser.parse_args()

modules/calib3d/include/opencv2/calib3d.hpp

@@ -29,6 +29,7 @@ static Mat homogeneousInverse(const Mat& T)
 // q = sin(theta/2) * v
 // theta - rotation angle
 // v     - unit rotation axis, |v| = 1
+// Reference: http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/
 static Mat rot2quatMinimal(const Mat& R)
 {
     CV_Assert(R.type() == CV_64FC1 && R.rows >= 3 && R.cols >= 3);
@@ -44,7 +45,7 @@ static Mat rot2quatMinimal(const Mat& R)
         qx = (m21 - m12) / S;
         qy = (m02 - m20) / S;
         qz = (m10 - m01) / S;
-    } else if ((m00 > m11)&(m00 > m22)) {
+    } else if (m00 > m11 && m00 > m22) {
         double S = sqrt(1.0 + m00 - m11 - m22) * 2; // S=4*qx
         qx = 0.25 * S;
         qy = (m01 + m10) / S;
@@ -98,6 +99,7 @@ static Mat quatMinimal2rot(const Mat& q)
 //
 // q - 4x1 unit quaternion <qw, qx, qy, qz>
 // R - 3x3 rotation matrix
+// Reference: http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/
 static Mat rot2quat(const Mat& R)
 {
     CV_Assert(R.type() == CV_64FC1 && R.rows >= 3 && R.cols >= 3);
@@ -114,7 +116,7 @@ static Mat rot2quat(const Mat& R)
         qx = (m21 - m12) / S;
         qy = (m02 - m20) / S;
         qz = (m10 - m01) / S;
-    } else if ((m00 > m11)&(m00 > m22)) {
+    } else if (m00 > m11 && m00 > m22) {
         double S = sqrt(1.0 + m00 - m11 - m22) * 2; // S=4*qx
         qw = (m21 - m12) / S;
         qx = 0.25 * S;
@@ -572,7 +574,11 @@ static void calibrateHandEyeAndreff(const std::vector<Mat>& Hg, const std::vecto
     R = R.reshape(1, 2, newSize);
     //Eq 15
     double det = determinant(R);
-    R = pow(sign_double(det) / abs(det), 1.0/3.0) * R;
+    if (std::fabs(det) < FLT_EPSILON)
+    {
+        CV_Error(Error::StsNoConv, "calibrateHandEye() with CALIB_HAND_EYE_ANDREFF method: determinant(R) is null");
+    }
+    R = cubeRoot(static_cast<float>(sign_double(det) / abs(det))) * R;
 
     Mat w, u, vt;
     SVDecomp(R, w, u, vt);

modules/calib3d/src/calibration_handeye.cpp

@@ -51,6 +51,8 @@
 
 #include "usac.hpp"
 
+#include <opencv2/core/utils/logger.hpp>
+
 namespace cv
 {
 #if defined _DEBUG || defined CV_STATIC_ANALYSIS
@@ -809,6 +811,15 @@ int solvePnPGeneric( InputArray _opoints, InputArray _ipoints,
     vector<Mat> vec_rvecs, vec_tvecs;
     if (flags == SOLVEPNP_EPNP || flags == SOLVEPNP_DLS || flags == SOLVEPNP_UPNP)
     {
+        if (flags == SOLVEPNP_DLS)
+        {
+            CV_LOG_DEBUG(NULL, "Broken implementation for SOLVEPNP_DLS. Fallback to EPnP.");
+        }
+        else if (flags == SOLVEPNP_UPNP)
+        {
+            CV_LOG_DEBUG(NULL, "Broken implementation for SOLVEPNP_UPNP. Fallback to EPnP.");
+        }
+
         Mat undistortedPoints;
         undistortPoints(ipoints, undistortedPoints, cameraMatrix, distCoeffs);
         epnp PnP(cameraMatrix, opoints, undistortedPoints);

modules/calib3d/src/solvepnp.cpp

@@ -7,6 +7,38 @@
 
 namespace opencv_test { namespace {
 
+static std::string getMethodName(HandEyeCalibrationMethod method)
+{
+    std::string method_name = "";
+    switch (method)
+    {
+    case CALIB_HAND_EYE_TSAI:
+        method_name = "Tsai";
+        break;
+
+    case CALIB_HAND_EYE_PARK:
+        method_name = "Park";
+        break;
+
+    case CALIB_HAND_EYE_HORAUD:
+        method_name = "Horaud";
+        break;
+
+    case CALIB_HAND_EYE_ANDREFF:
+        method_name = "Andreff";
+        break;
+
+    case CALIB_HAND_EYE_DANIILIDIS:
+        method_name = "Daniilidis";
+        break;
+
+    default:
+        break;
+    }
+
+    return method_name;
+}
+
 class CV_CalibrateHandEyeTest : public cvtest::BaseTest
 {
 public:
@@ -48,7 +80,6 @@ class CV_CalibrateHandEyeTest : public cvtest::BaseTest
                       std::vector<Mat> &R_target2cam, std::vector<Mat> &t_target2cam,
                       bool noise, Mat& R_cam2gripper, Mat& t_cam2gripper);
     Mat homogeneousInverse(const Mat& T);
-    std::string getMethodName(HandEyeCalibrationMethod method);
     double sign_double(double val);
 
     double eps_rvec[5];
@@ -340,45 +371,95 @@ Mat CV_CalibrateHandEyeTest::homogeneousInverse(const Mat& T)
     return Tinv;
 }
 
-std::string CV_CalibrateHandEyeTest::getMethodName(HandEyeCalibrationMethod method)
+double CV_CalibrateHandEyeTest::sign_double(double val)
 {
-    std::string method_name = "";
-    switch (method)
-    {
-    case CALIB_HAND_EYE_TSAI:
-        method_name = "Tsai";
-        break;
+    return (0 < val) - (val < 0);
+}
 
-    case CALIB_HAND_EYE_PARK:
-        method_name = "Park";
-        break;
+///////////////////////////////////////////////////////////////////////////////////////////////////
 
-    case CALIB_HAND_EYE_HORAUD:
-        method_name = "Horaud";
-        break;
+TEST(Calib3d_CalibrateHandEye, regression) { CV_CalibrateHandEyeTest test; test.safe_run(); }
 
-    case CALIB_HAND_EYE_ANDREFF:
-        method_name = "Andreff";
-        break;
+TEST(Calib3d_CalibrateHandEye, regression_17986)
+{
+    const std::string camera_poses_filename = findDataFile("cv/hand_eye_calibration/cali.txt");
+    const std::string end_effector_poses = findDataFile("cv/hand_eye_calibration/robot_cali.txt");
 
-    case CALIB_HAND_EYE_DANIILIDIS:
-        method_name = "Daniilidis";
-        break;
+    std::vector<Mat> R_target2cam;
+    std::vector<Mat> t_target2cam;
+    // Parse camera poses
+    {
+        std::ifstream file(camera_poses_filename.c_str());
+        ASSERT_TRUE(file.is_open());
+
+        int ndata = 0;
+        file >> ndata;
+        R_target2cam.reserve(ndata);
+        t_target2cam.reserve(ndata);
+
+        std::string image_name;
+        Matx33d cameraMatrix;
+        Matx33d R;
+        Matx31d t;
+        Matx16d distCoeffs;
+        Matx13d distCoeffs2;
+        while (file >> image_name >>
+               cameraMatrix(0,0) >> cameraMatrix(0,1) >> cameraMatrix(0,2) >>
+               cameraMatrix(1,0) >> cameraMatrix(1,1) >> cameraMatrix(1,2) >>
+               cameraMatrix(2,0) >> cameraMatrix(2,1) >> cameraMatrix(2,2) >>
+               R(0,0) >> R(0,1) >> R(0,2) >>
+               R(1,0) >> R(1,1) >> R(1,2) >>
+               R(2,0) >> R(2,1) >> R(2,2) >>
+               t(0) >> t(1) >> t(2) >>
+               distCoeffs(0) >> distCoeffs(1) >> distCoeffs(2) >> distCoeffs(3) >> distCoeffs(4) >>
+               distCoeffs2(0) >> distCoeffs2(1) >> distCoeffs2(2)) {
+            R_target2cam.push_back(Mat(R));
+            t_target2cam.push_back(Mat(t));
+        }
+    }
 
-    default:
-        break;
+    std::vector<Mat> R_gripper2base;
+    std::vector<Mat> t_gripper2base;
+    // Parse end-effector poses
+    {
+        std::ifstream file(end_effector_poses.c_str());
+        ASSERT_TRUE(file.is_open());
+
+        int ndata = 0;
+        file >> ndata;
+        R_gripper2base.reserve(ndata);
+        t_gripper2base.reserve(ndata);
+
+        Matx33d R;
+        Matx31d t;
+        Matx14d last_row;
+        while (file >>
+               R(0,0) >> R(0,1) >> R(0,2) >> t(0) >>
+               R(1,0) >> R(1,1) >> R(1,2) >> t(1) >>
+               R(2,0) >> R(2,1) >> R(2,2) >> t(2) >>
+               last_row(0) >> last_row(1) >> last_row(2) >> last_row(3)) {
+            R_gripper2base.push_back(Mat(R));
+            t_gripper2base.push_back(Mat(t));
+        }
     }
 
-    return method_name;
-}
+    std::vector<HandEyeCalibrationMethod> methods;
+    methods.push_back(CALIB_HAND_EYE_TSAI);
+    methods.push_back(CALIB_HAND_EYE_PARK);
+    methods.push_back(CALIB_HAND_EYE_HORAUD);
+    methods.push_back(CALIB_HAND_EYE_ANDREFF);
+    methods.push_back(CALIB_HAND_EYE_DANIILIDIS);
 
-double CV_CalibrateHandEyeTest::sign_double(double val)
-{
-    return (0 < val) - (val < 0);
-}
+    for (size_t idx = 0; idx < methods.size(); idx++) {
+        SCOPED_TRACE(cv::format("method=%s", getMethodName(methods[idx]).c_str()));
 
-///////////////////////////////////////////////////////////////////////////////////////////////////
+        Matx33d R_cam2gripper_est;
+        Matx31d t_cam2gripper_est;
+        calibrateHandEye(R_gripper2base, t_gripper2base, R_target2cam, t_target2cam, R_cam2gripper_est, t_cam2gripper_est, methods[idx]);
 
-TEST(Calib3d_CalibrateHandEye, regression) { CV_CalibrateHandEyeTest test; test.safe_run(); }
+        EXPECT_TRUE(checkRange(R_cam2gripper_est));
+        EXPECT_TRUE(checkRange(t_cam2gripper_est));
+    }
+}
 
 }} // namespace

modules/calib3d/test/test_calibration_hand_eye.cpp

@@ -16,6 +16,7 @@ Implementation of Scale layer.
 #include "../op_inf_engine.hpp"
 #include "../ie_ngraph.hpp"
 
+#include <opencv2/imgproc.hpp>
 #include <opencv2/dnn/shape_utils.hpp>
 
 #ifdef HAVE_CUDA
@@ -389,7 +390,7 @@ class DataAugmentationLayerImpl CV_FINAL : public DataAugmentationLayer
                          std::vector<MatShape> &internals) const CV_OVERRIDE
     {
         CV_Assert_N(inputs.size() == 1, blobs.size() == 3);
-        CV_Assert_N(blobs[0].total() == 1, blobs[1].total() == total(inputs[0], 1),
+        CV_Assert_N(blobs[0].total() == 1,
                     blobs[2].total() == inputs[0][1]);
 
         outputs.assign(1, inputs[0]);
@@ -412,15 +413,20 @@ class DataAugmentationLayerImpl CV_FINAL : public DataAugmentationLayer
         float* outData = outputs[0].ptr<float>();
 
         Mat data_mean_cpu = blobs[1].clone();
+        Mat mean_resize = Mat(inputs[0].size[3], inputs[0].size[2], CV_32FC3);
+        Mat mean_3d = Mat(data_mean_cpu.size[3], data_mean_cpu.size[2], CV_32FC3, data_mean_cpu.ptr<float>(0));
+        resize(mean_3d, mean_resize, Size(inputs[0].size[3], inputs[0].size[2]));
+        int new_size[] = {1, mean_resize.channels(), mean_resize.cols, mean_resize.rows};
+        Mat data_mean_cpu_resize = mean_resize.reshape(1, *new_size);
         Mat data_mean_per_channel_cpu = blobs[2].clone();
 
-        const int numWeights = data_mean_cpu.total();
+        const int numWeights = data_mean_cpu_resize.total();
         CV_Assert(numWeights != 0);
 
         ++num_iter;
         if (num_iter <= recompute_mean)
         {
-            data_mean_cpu *= (num_iter - 1);
+            data_mean_cpu_resize *= (num_iter - 1);
             const int batch = inputs[0].size[0];
             float alpha = 1.0 / batch;
 
@@ -429,15 +435,15 @@ class DataAugmentationLayerImpl CV_FINAL : public DataAugmentationLayer
                 Mat inpSlice(1, numWeights, CV_32F, inpData);
                 inpSlice = alpha * inpSlice;
 
-                add(data_mean_cpu.reshape(1, 1), inpSlice, data_mean_cpu.reshape(1, 1));
+                add(data_mean_cpu_resize.reshape(1, 1), inpSlice, data_mean_cpu_resize.reshape(1, 1));
                 inpData += numWeights;
             }
-            data_mean_cpu *= (1.0 / num_iter);
+            data_mean_cpu_resize *= (1.0 / num_iter);
 
-            int newsize[] = {blobs[1].size[1], (int)blobs[1].total(2)};
-            reduce(data_mean_cpu.reshape(1, 2, &newsize[0]), data_mean_per_channel_cpu, 1, REDUCE_SUM, CV_32F);
+            int newsize[] = {inputs[0].size[1], (int)inputs[0].total(2)};
+            reduce(data_mean_cpu_resize.reshape(1, 2, &newsize[0]), data_mean_per_channel_cpu, 1, REDUCE_SUM, CV_32F);
 
-            int area = blobs[1].total(2);
+            int area = inputs[0].total(2);
             data_mean_per_channel_cpu *= (1.0 / area);
         }
 
@@ -452,7 +458,7 @@ class DataAugmentationLayerImpl CV_FINAL : public DataAugmentationLayer
                 Mat inpSlice(1, numWeights, CV_32F, inpData);
                 Mat outSlice(1, numWeights, CV_32F, outData);
 
-                add(inpSlice, (-1) * data_mean_cpu, outSlice);
+                add(inpSlice, (-1) * data_mean_cpu_resize, outSlice);
                 inpData += numWeights;
                 outData += numWeights;
             }