From 2b67bc448d2b74103a0202b4bfa0342175df8dfb Mon Sep 17 00:00:00 2001
From: Victor <kataev.victor.1234@gmail.com>
Date: Thu, 12 May 2022 11:56:58 +0300
Subject: [PATCH] Merge pull request #21931 from
 victor1234:calib3d-add-undistortImagePoints

Add undistortImagePoints function

* Add undistortImagePoints function
undistortPoints has unclear interface and additional functionality. New function computes only undistorted image points position

* Add undistortImagePoints test

* Add TermCriteria

* Fix layout
---
 modules/calib3d/include/opencv2/calib3d.hpp   | 16 ++++++++
 modules/calib3d/src/undistort.dispatch.cpp    |  6 +++
 .../calib3d/test/test_undistort_points.cpp    | 38 +++++++++++++++++++
 3 files changed, 60 insertions(+)

diff --git a/modules/calib3d/include/opencv2/calib3d.hpp b/modules/calib3d/include/opencv2/calib3d.hpp
index 5fe7fb9596fa..ad3527aa423b 100644
--- a/modules/calib3d/include/opencv2/calib3d.hpp
+++ b/modules/calib3d/include/opencv2/calib3d.hpp
@@ -45,6 +45,7 @@
 #define OPENCV_CALIB3D_HPP
 
 #include "opencv2/core.hpp"
+#include "opencv2/core/types.hpp"
 #include "opencv2/features2d.hpp"
 #include "opencv2/core/affine.hpp"
 
@@ -3721,6 +3722,21 @@ void undistortPoints(InputArray src, OutputArray dst,
                      InputArray cameraMatrix, InputArray distCoeffs,
                      InputArray R, InputArray P, TermCriteria criteria);
 
+/**
+ * @brief Compute undistorted image points position
+ *
+ * @param src Observed points position, 2xN/Nx2 1-channel or 1xN/Nx1 2-channel (CV_32FC2 or
+CV_64FC2) (or vector\<Point2f\> ).
+ * @param dst Output undistorted points position (1xN/Nx1 2-channel or vector\<Point2f\> ).
+ * @param cameraMatrix Camera matrix \f$\vecthreethree{f_x}{0}{c_x}{0}{f_y}{c_y}{0}{0}{1}\f$ .
+ * @param distCoeffs Distortion coefficients
+ */
+CV_EXPORTS_W
+void undistortImagePoints(InputArray src, OutputArray dst, InputArray cameraMatrix,
+                          InputArray distCoeffs,
+                          TermCriteria = TermCriteria(TermCriteria::MAX_ITER + TermCriteria::EPS, 5,
+                                                      0.01));
+
 //! @} calib3d
 
 /** @brief The methods in this namespace use a so-called fisheye camera model.
diff --git a/modules/calib3d/src/undistort.dispatch.cpp b/modules/calib3d/src/undistort.dispatch.cpp
index 146befd955f0..6c3d32941c37 100644
--- a/modules/calib3d/src/undistort.dispatch.cpp
+++ b/modules/calib3d/src/undistort.dispatch.cpp
@@ -40,6 +40,7 @@
 //
 //M*/
 
+#include "opencv2/core/types.hpp"
 #include "precomp.hpp"
 #include "distortion_model.hpp"
 
@@ -607,6 +608,11 @@ void undistortPoints(InputArray _src, OutputArray _dst,
     cvUndistortPointsInternal(&_csrc, &_cdst, &_ccameraMatrix, pD, pR, pP, criteria);
 }
 
+void undistortImagePoints(InputArray src, OutputArray dst, InputArray cameraMatrix, InputArray distCoeffs, TermCriteria termCriteria)
+{
+    undistortPoints(src, dst, cameraMatrix, distCoeffs, noArray(), cameraMatrix, termCriteria);
+}
+
 static Point2f mapPointSpherical(const Point2f& p, float alpha, Vec4d* J, enum UndistortTypes projType)
 {
     double x = p.x, y = p.y;
diff --git a/modules/calib3d/test/test_undistort_points.cpp b/modules/calib3d/test/test_undistort_points.cpp
index 962ff03cca40..f92bec068b89 100644
--- a/modules/calib3d/test/test_undistort_points.cpp
+++ b/modules/calib3d/test/test_undistort_points.cpp
@@ -3,6 +3,7 @@
 // of this distribution and at http://opencv.org/license.html.
 
 #include <opencv2/ts/cuda_test.hpp> // EXPECT_MAT_NEAR
+#include "opencv2/core/types.hpp"
 #include "test_precomp.hpp"
 
 namespace opencv_test { namespace {
@@ -97,6 +98,43 @@ TEST_F(UndistortPointsTest, accuracy)
     }
 }
 
+TEST_F(UndistortPointsTest, undistortImagePointsAccuracy)
+{
+    Mat intrinsics, distCoeffs;
+    generateCameraMatrix(intrinsics);
+
+    vector<Point3f> points(500);
+    generate3DPointCloud(points);
+
+
+    int modelMembersCount[] = {4,5,8};
+    for (int idx = 0; idx < 3; idx++)
+    {
+        generateDistCoeffs(distCoeffs, modelMembersCount[idx]);
+
+        /* Project points with distortion */
+        vector<Point2f> projectedPoints;
+        projectPoints(Mat(points), Mat::zeros(3,1,CV_64FC1),
+                      Mat::zeros(3,1,CV_64FC1), intrinsics,
+                      distCoeffs, projectedPoints);
+
+        /* Project points without distortion */
+        vector<Point2f> realUndistortedPoints;
+        projectPoints(Mat(points), Mat::zeros(3, 1, CV_64FC1),
+                      Mat::zeros(3,1,CV_64FC1), intrinsics,
+                      Mat::zeros(4,1,CV_64FC1), realUndistortedPoints);
+
+        /* Undistort points */
+        Mat undistortedPoints;
+        TermCriteria termCriteria(TermCriteria::MAX_ITER + TermCriteria::EPS, 5, thresh / 2);
+        undistortImagePoints(Mat(projectedPoints), undistortedPoints, intrinsics, distCoeffs,
+                             termCriteria);
+
+        EXPECT_MAT_NEAR(realUndistortedPoints, undistortedPoints.t(), thresh);
+    }
+}
+
+
 TEST_F(UndistortPointsTest, stop_criteria)
 {
     Mat cameraMatrix = (Mat_<double>(3,3,CV_64F) << 857.48296979, 0, 968.06224829,