From 7b3a752012f0d8e54c32eeeb9362adecfd4d7c47 Mon Sep 17 00:00:00 2001
From: Vitaly Tuzov <terfendail@mediana.jetos.com>
Date: Tue, 10 Sep 2019 11:40:14 +0300
Subject: [PATCH] Fixed universal intrinsic undistort() implementation

---
 modules/calib3d/test/test_undistort.cpp | 38 +++++++++++++++++++++++++
 modules/imgproc/src/undistort.simd.hpp  | 21 ++++++++------
 2 files changed, 51 insertions(+), 8 deletions(-)

diff --git a/modules/calib3d/test/test_undistort.cpp b/modules/calib3d/test/test_undistort.cpp
index 42ad294e51..4d90ecab4d 100644
--- a/modules/calib3d/test/test_undistort.cpp
+++ b/modules/calib3d/test/test_undistort.cpp
@@ -1114,6 +1114,44 @@ TEST(Calib3d_UndistortPoints, outputShape)
     }
 }
 
+TEST(Imgproc_undistort, regression_15286)
+{
+    double kmat_data[9] = { 3217, 0, 1592, 0, 3217, 1201, 0, 0, 1 };
+    Mat kmat(3, 3, CV_64F, kmat_data);
+    double dist_coeff_data[5] = { 0.04, -0.4, -0.01, 0.04, 0.7 };
+    Mat dist_coeffs(5, 1, CV_64F, dist_coeff_data);
+
+    Mat img = Mat::zeros(512, 512, CV_8UC1);
+    img.at<uchar>(128, 128) = 255;
+    img.at<uchar>(128, 384) = 255;
+    img.at<uchar>(384, 384) = 255;
+    img.at<uchar>(384, 128) = 255;
+
+    Mat ref = Mat::zeros(512, 512, CV_8UC1);
+    ref.at<uchar>(Point(24, 98)) = 78;
+    ref.at<uchar>(Point(24, 99)) = 114;
+    ref.at<uchar>(Point(25, 98)) = 36;
+    ref.at<uchar>(Point(25, 99)) = 60;
+    ref.at<uchar>(Point(27, 361)) = 6;
+    ref.at<uchar>(Point(28, 361)) = 188;
+    ref.at<uchar>(Point(28, 362)) = 49;
+    ref.at<uchar>(Point(29, 361)) = 44;
+    ref.at<uchar>(Point(29, 362)) = 16;
+    ref.at<uchar>(Point(317, 366)) = 134;
+    ref.at<uchar>(Point(317, 367)) = 78;
+    ref.at<uchar>(Point(318, 366)) = 40;
+    ref.at<uchar>(Point(318, 367)) = 29;
+    ref.at<uchar>(Point(310, 104)) = 106;
+    ref.at<uchar>(Point(310, 105)) = 30;
+    ref.at<uchar>(Point(311, 104)) = 112;
+    ref.at<uchar>(Point(311, 105)) = 38;
+
+    Mat img_undist;
+    undistort(img, img_undist, kmat, dist_coeffs);
+
+    ASSERT_EQ(0.0, cvtest::norm(img_undist, ref, cv::NORM_INF));
+}
+
 TEST(Calib3d_initUndistortRectifyMap, regression_14467)
 {
     Size size_w_h(512 + 3, 512);
diff --git a/modules/imgproc/src/undistort.simd.hpp b/modules/imgproc/src/undistort.simd.hpp
index 20ca545fdb..7998a3b086 100644
--- a/modules/imgproc/src/undistort.simd.hpp
+++ b/modules/imgproc/src/undistort.simd.hpp
@@ -153,19 +153,24 @@ public:
                 m_2 = vx_setall_f64(k5);
                 m_0 /= v_muladd(v_muladd(v_muladd(m_3, r2_0, m_2), r2_0, vx_setall_f64(k4)), r2_0, v_one);
                 m_1 /= v_muladd(v_muladd(v_muladd(m_3, r2_1, m_2), r2_1, vx_setall_f64(k4)), r2_1, v_one);
+
+                m_3 = vx_setall_f64(2.0);
+                xd_0 = v_muladd(m_3, xd_0, r2_0);
+                yd_0 = v_muladd(m_3, yd_0, r2_0);
+                xd_1 = v_muladd(m_3, xd_1, r2_1);
+                yd_1 = v_muladd(m_3, yd_1, r2_1);
+                m_2 = x_0 * y_0 * m_3;
+                m_3 = x_1 * y_1 * m_3;
+
                 x_0 *= m_0; y_0 *= m_0; x_1 *= m_1; y_1 *= m_1;
 
                 m_0 = vx_setall_f64(p1);
                 m_1 = vx_setall_f64(p2);
-                m_2 = vx_setall_f64(2.0);
-                xd_0 = v_muladd(v_muladd(m_2, xd_0, r2_0), m_1, x_0);
-                yd_0 = v_muladd(v_muladd(m_2, yd_0, r2_0), m_0, y_0);
-                xd_1 = v_muladd(v_muladd(m_2, xd_1, r2_1), m_1, x_1);
-                yd_1 = v_muladd(v_muladd(m_2, yd_1, r2_1), m_0, y_1);
+                xd_0 = v_muladd(xd_0, m_1, x_0);
+                yd_0 = v_muladd(yd_0, m_0, y_0);
+                xd_1 = v_muladd(xd_1, m_1, x_1);
+                yd_1 = v_muladd(yd_1, m_0, y_1);
 
-                m_0 *= m_2; m_1 *= m_2;
-                m_2 = x_0 * y_0;
-                m_3 = x_1 * y_1;
                 xd_0 = v_muladd(m_0, m_2, xd_0);
                 yd_0 = v_muladd(m_1, m_2, yd_0);
                 xd_1 = v_muladd(m_0, m_3, xd_1);