From 72f35e0626c2a4a87d4617ad4a8a6b1ff3b2fb09 Mon Sep 17 00:00:00 2001 From: Alexander Alekhin Date: Thu, 5 Dec 2019 15:36:39 +0300 Subject: [PATCH] Merge pull request #16052 from alalek:issue_16040 * calib3d: use normalized input in solvePnPGeneric() * calib3d: java regression test for solvePnPGeneric * calib3d: python regression test for solvePnPGeneric --- modules/calib3d/misc/java/test/Calib3dTest.java | 43 ++++++++++++++++++++++ modules/calib3d/misc/python/test/test_solvepnp.py | 44 +++++++++++++++++++++++ modules/calib3d/src/solvepnp.cpp | 26 +++++++------- modules/core/src/norm.cpp | 3 +- 4 files changed, 101 insertions(+), 15 deletions(-) create mode 100644 modules/calib3d/misc/python/test/test_solvepnp.py diff --git a/modules/calib3d/misc/java/test/Calib3dTest.java b/modules/calib3d/misc/java/test/Calib3dTest.java index e9bb6ba..b474fc7 100644 --- a/modules/calib3d/misc/java/test/Calib3dTest.java +++ b/modules/calib3d/misc/java/test/Calib3dTest.java @@ -1,5 +1,7 @@ package org.opencv.test.calib3d; +import java.util.ArrayList; + import org.opencv.calib3d.Calib3d; import org.opencv.core.CvType; import org.opencv.core.Mat; @@ -639,4 +641,45 @@ public class Calib3dTest extends OpenCVTestCase { assertEquals((1 << 17), Calib3d.CALIB_USE_LU); assertEquals((1 << 22), Calib3d.CALIB_USE_EXTRINSIC_GUESS); } + + public void testSolvePnPGeneric_regression_16040() { + Mat intrinsics = Mat.eye(3, 3, CvType.CV_64F); + intrinsics.put(0, 0, 400); + intrinsics.put(1, 1, 400); + intrinsics.put(0, 2, 640 / 2); + intrinsics.put(1, 2, 480 / 2); + + final int minPnpPointsNum = 4; + + MatOfPoint3f points3d = new MatOfPoint3f(); + points3d.alloc(minPnpPointsNum); + MatOfPoint2f points2d = new MatOfPoint2f(); + points2d.alloc(minPnpPointsNum); + + for (int i = 0; i < minPnpPointsNum; i++) { + double x = Math.random() * 100 - 50; + double y = Math.random() * 100 - 50; + points2d.put(i, 0, x, y); //add(new Point(x, y)); + points3d.put(i, 0, 0, y, x); // add(new Point3(0, y, x)); + } + + ArrayList rvecs = new ArrayList(); + ArrayList tvecs = new ArrayList(); + + Mat rvec = new Mat(); + Mat tvec = new Mat(); + + Mat reprojectionError = new Mat(2, 1, CvType.CV_64FC1); + + Calib3d.solvePnPGeneric(points3d, points2d, intrinsics, new MatOfDouble(), rvecs, tvecs, false, Calib3d.SOLVEPNP_IPPE, rvec, tvec, reprojectionError); + + Mat truth_rvec = new Mat(3, 1, CvType.CV_64F); + truth_rvec.put(0, 0, 0, Math.PI / 2, 0); + + Mat truth_tvec = new Mat(3, 1, CvType.CV_64F); + truth_tvec.put(0, 0, -320, -240, 400); + + assertMatEqual(truth_rvec, rvecs.get(0), 10 * EPS); + assertMatEqual(truth_tvec, tvecs.get(0), 1000 * EPS); + } } diff --git a/modules/calib3d/misc/python/test/test_solvepnp.py b/modules/calib3d/misc/python/test/test_solvepnp.py new file mode 100644 index 0000000..b751e15 --- /dev/null +++ b/modules/calib3d/misc/python/test/test_solvepnp.py @@ -0,0 +1,44 @@ +#!/usr/bin/env python +# Python 2/3 compatibility +from __future__ import print_function + +import numpy as np +import cv2 as cv + +from tests_common import NewOpenCVTests + +class solvepnp_test(NewOpenCVTests): + + def test_regression_16040(self): + obj_points = np.array([[0, 0, 0], [0, 1, 0], [1, 1, 0], [1, 0, 0]], dtype=np.float32) + img_points = np.array( + [[700, 400], [700, 600], [900, 600], [900, 400]], dtype=np.float32 + ) + + cameraMatrix = np.array( + [[712.0634, 0, 800], [0, 712.540, 500], [0, 0, 1]], dtype=np.float32 + ) + distCoeffs = np.array([[0, 0, 0, 0]], dtype=np.float32) + r = np.array([], dtype=np.float32) + x, r, t, e = cv.solvePnPGeneric( + obj_points, img_points, cameraMatrix, distCoeffs, reprojectionError=r + ) + + def test_regression_16040_2(self): + obj_points = np.array([[0, 0, 0], [0, 1, 0], [1, 1, 0], [1, 0, 0]], dtype=np.float32) + img_points = np.array( + [[[700, 400], [700, 600], [900, 600], [900, 400]]], dtype=np.float32 + ) + + cameraMatrix = np.array( + [[712.0634, 0, 800], [0, 712.540, 500], [0, 0, 1]], dtype=np.float32 + ) + distCoeffs = np.array([[0, 0, 0, 0]], dtype=np.float32) + r = np.array([], dtype=np.float32) + x, r, t, e = cv.solvePnPGeneric( + obj_points, img_points, cameraMatrix, distCoeffs, reprojectionError=r + ) + + +if __name__ == '__main__': + NewOpenCVTests.bootstrap() diff --git a/modules/calib3d/src/solvepnp.cpp b/modules/calib3d/src/solvepnp.cpp index 62ce0bd..d2fb397 100644 --- a/modules/calib3d/src/solvepnp.cpp +++ b/modules/calib3d/src/solvepnp.cpp @@ -753,10 +753,8 @@ int solvePnPGeneric( InputArray _opoints, InputArray _ipoints, CV_Assert( ( (npoints >= 4) || (npoints == 3 && flags == SOLVEPNP_ITERATIVE && useExtrinsicGuess) ) && npoints == std::max(ipoints.checkVector(2, CV_32F), ipoints.checkVector(2, CV_64F)) ); - if (opoints.cols == 3) - opoints = opoints.reshape(3); - if (ipoints.cols == 2) - ipoints = ipoints.reshape(2); + opoints = opoints.reshape(3, npoints); + ipoints = ipoints.reshape(2, npoints); if( flags != SOLVEPNP_ITERATIVE ) useExtrinsicGuess = false; @@ -796,7 +794,7 @@ int solvePnPGeneric( InputArray _opoints, InputArray _ipoints, else if (flags == SOLVEPNP_P3P || flags == SOLVEPNP_AP3P) { vector rvecs, tvecs; - solveP3P(_opoints, _ipoints, _cameraMatrix, _distCoeffs, rvecs, tvecs, flags); + solveP3P(opoints, ipoints, _cameraMatrix, _distCoeffs, rvecs, tvecs, flags); vec_rvecs.insert(vec_rvecs.end(), rvecs.begin(), rvecs.end()); vec_tvecs.insert(vec_tvecs.end(), tvecs.begin(), tvecs.end()); } @@ -1017,37 +1015,37 @@ int solvePnPGeneric( InputArray _opoints, InputArray _ipoints, "Type of reprojectionError must be CV_32FC1 or CV_64FC1!"); Mat objectPoints, imagePoints; - if (_opoints.depth() == CV_32F) + if (opoints.depth() == CV_32F) { - _opoints.getMat().convertTo(objectPoints, CV_64F); + opoints.convertTo(objectPoints, CV_64F); } else { - objectPoints = _opoints.getMat(); + objectPoints = opoints; } - if (_ipoints.depth() == CV_32F) + if (ipoints.depth() == CV_32F) { - _ipoints.getMat().convertTo(imagePoints, CV_64F); + ipoints.convertTo(imagePoints, CV_64F); } else { - imagePoints = _ipoints.getMat(); + imagePoints = ipoints; } for (size_t i = 0; i < vec_rvecs.size(); i++) { vector projectedPoints; projectPoints(objectPoints, vec_rvecs[i], vec_tvecs[i], cameraMatrix, distCoeffs, projectedPoints); - double rmse = norm(projectedPoints, imagePoints, NORM_L2) / sqrt(2*projectedPoints.size()); + double rmse = norm(Mat(projectedPoints, false), imagePoints, NORM_L2) / sqrt(2*projectedPoints.size()); Mat err = reprojectionError.getMat(); if (type == CV_32F) { - err.at(0,static_cast(i)) = static_cast(rmse); + err.at(static_cast(i)) = static_cast(rmse); } else { - err.at(0,static_cast(i)) = rmse; + err.at(static_cast(i)) = rmse; } } } diff --git a/modules/core/src/norm.cpp b/modules/core/src/norm.cpp index ff17186..148c25a 100644 --- a/modules/core/src/norm.cpp +++ b/modules/core/src/norm.cpp @@ -1062,7 +1062,8 @@ double cv::norm( InputArray _src1, InputArray _src2, int normType, InputArray _m { CV_INSTRUMENT_REGION(); - CV_Assert( _src1.sameSize(_src2) && _src1.type() == _src2.type() ); + CV_CheckTypeEQ(_src1.type(), _src2.type(), "Input type mismatch"); + CV_Assert(_src1.sameSize(_src2)); #if defined HAVE_OPENCL || defined HAVE_IPP double _result = 0; -- 2.7.4