From e90935e81cd83973af7b5cb15c770a210c565e10 Mon Sep 17 00:00:00 2001
From: j3knk <96484997+j3knk@users.noreply.github.com>
Date: Mon, 15 Jul 2024 14:10:08 +0200
Subject: [PATCH] Merge pull request #25824 from
 j3knk:calib3d/fix_projectpoints

calib3d: fix Rodrigues CV_32F and CV_64F type mismatch in projectPoints #25824

Fixes #25318

### Pull Request Readiness Checklist

See details at https://github.com/opencv/opencv/wiki/How_to_contribute#making-a-good-pull-request

- [x] I agree to contribute to the project under Apache 2 License.
- [x] To the best of my knowledge, the proposed patch is not based on a code under GPL or another license that is incompatible with OpenCV
- [x] The PR is proposed to the proper branch
- [x] There is a reference to the original bug report and related work
- [ ] There is accuracy test, performance test and test data in opencv_extra repository, if applicable
      Patch to opencv_extra has the same branch name.
- [ ] The feature is well documented and sample code can be built with the project CMake
---
 .../calib3d/misc/python/test/test_calibration.py  | 15 +++++++++++++++
 modules/calib3d/src/calibration.cpp               | 10 ++++++----
 2 files changed, 21 insertions(+), 4 deletions(-)

diff --git a/modules/calib3d/misc/python/test/test_calibration.py b/modules/calib3d/misc/python/test/test_calibration.py
index f2cf4cc493..c3093ce2ef 100644
--- a/modules/calib3d/misc/python/test/test_calibration.py
+++ b/modules/calib3d/misc/python/test/test_calibration.py
@@ -67,7 +67,22 @@ class calibration_test(NewOpenCVTests):
         self.assertLess(cv.norm(camera_matrix - cameraMatrixTest, cv.NORM_L1), normCamEps)
         self.assertLess(cv.norm(dist_coefs - distCoeffsTest, cv.NORM_L1), normDistEps)
 
+    def test_projectPoints(self):
+        objectPoints = np.array([[181.24588 ,  87.80361 ,  11.421074],
+            [ 87.17948 , 184.75563 ,  37.223446],
+            [ 22.558456,  45.495266, 246.05797 ]], dtype=np.float32)
+        rvec = np.array([[ 0.9357548 , -0.28316498,  0.21019171],
+            [ 0.30293274,  0.9505806 , -0.06803132],
+            [-0.18054008,  0.12733458,  0.9752903 ]], dtype=np.float32)
+        tvec = np.array([ 69.32692 ,  17.602057, 135.77672 ], dtype=np.float32)
+        cameraMatrix = np.array([[214.0047  ,  26.98735 , 253.37799 ],
+            [189.8172  ,  10.038101,  18.862494],
+            [114.07123 , 200.87277 , 194.56332 ]], dtype=np.float32)
+        distCoeffs = distCoeffs = np.zeros((4, 1), dtype=np.float32)
 
+        imagePoints, jacobian = cv.projectPoints(objectPoints, rvec, tvec, cameraMatrix, distCoeffs)
+        self.assertTrue(imagePoints is not None)
+        self.assertTrue(jacobian is not None)
 
 if __name__ == '__main__':
     NewOpenCVTests.bootstrap()
diff --git a/modules/calib3d/src/calibration.cpp b/modules/calib3d/src/calibration.cpp
index c428da6bd5..9729cb7bea 100644
--- a/modules/calib3d/src/calibration.cpp
+++ b/modules/calib3d/src/calibration.cpp
@@ -538,11 +538,11 @@ static void cvProjectPoints2Internal( const CvMat* objectPoints,
     int calc_derivatives;
     const CvPoint3D64f* M;
     CvPoint2D64f* m;
-    double r[3], R[9], dRdr[27], t[3], a[9], k[14] = {0,0,0,0,0,0,0,0,0,0,0,0,0,0}, fx, fy, cx, cy;
+    double r[3], R[9], R_vec[9], dRdr[27], t[3], a[9], k[14] = {0,0,0,0,0,0,0,0,0,0,0,0,0,0}, fx, fy, cx, cy;
     Matx33d matTilt = Matx33d::eye();
     Matx33d dMatTiltdTauX(0,0,0,0,0,0,0,-1,0);
     Matx33d dMatTiltdTauY(0,0,0,0,0,0,1,0,0);
-    CvMat _r, _t, _a = cvMat( 3, 3, CV_64F, a ), _k;
+    CvMat _r, _r_vec, _t, _a = cvMat( 3, 3, CV_64F, a ), _k;
     CvMat matR = cvMat( 3, 3, CV_64F, R ), _dRdr = cvMat( 3, 9, CV_64F, dRdr );
     double *dpdr_p = 0, *dpdt_p = 0, *dpdk_p = 0, *dpdf_p = 0, *dpdc_p = 0;
     double* dpdo_p = 0;
@@ -593,9 +593,11 @@ static void cvProjectPoints2Internal( const CvMat* objectPoints,
     if( r_vec->rows == 3 && r_vec->cols == 3 )
     {
         _r = cvMat( 3, 1, CV_64FC1, r );
-        cvRodrigues2( r_vec, &_r );
+        _r_vec = cvMat( r_vec->rows, r_vec->cols, CV_MAKETYPE(CV_64F,CV_MAT_CN(r_vec->type)), R_vec );
+        cvConvert( r_vec, &_r_vec );
+        cvRodrigues2( &_r_vec, &_r );
         cvRodrigues2( &_r, &matR, &_dRdr );
-        cvCopy( r_vec, &matR );
+        cvCopy( &_r_vec, &matR );
     }
     else
     {